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SYH0P3IS 


Tim prm&mit mrk iatt®fipt» JUmverat solutions for 
majnipulstors uitti rsdundiiiit lii&s. Amh protlff» feaw 
te«®» trfiiiiltloiMlly solved usiisit ©itiisr sow® ©ptlsilsatlott 
tsclmlqwa or pd®udi>»iiiv®r®®« of IMs wjyi; 

pr®s«fiits s now nepprosch by nodellltig tli® woir^spse® tdtii 
olastselss witis mipset tp thm imilpiilstor« 'Hi® 
hss &«®ii roprosMitsd m& m fi«ti«orlc» 4ii slgorilte is 
presented liiielji finds m mt« eonfigurutiofi of tti® etwiipir* 
lator by foHowinif a colllftlon fr«® path tdbiwigh tb® 
networlc* This path is first represtmtsd as a nootti ourm 
f rctt ®nd^»ff«etor to th# ban®* on th® linlisii 

th® aanipuiator joints. ar« than located as ol&m m possible 
to the curve* Hire® differimt pnoblms are disimsMd 
Uluatrutlng th® salient points of this slgorilte* 


CiWfEii I 


mtiiDmr.mfi 


1.1 

Robots mm toeing tiaed, to Iwodl® |j»r®iisd«£»lf 

I 

taak® toy by toy* Most q£ tfes rsgular ^obs n®ed ifjto ttw 
or six degr##8 of frosdoa muiipulators* Honowri my 
situMtions iSmmM a noiiipulator vith iBoro mm&mswm iit»ility« 
For oxmpla* in m spray painting rotjot* wbtro the omritiot 
ol a car tedy are to be mcmm»dg it is neoeosary tt&% Um 
iianipiilator ^uld h&m extra degrees of frettofi Cor redun^ 
tosioies)* 'itated in sisapler terra* if tbe mnipiilitor &m 
hoe to oirciiinimt seme obetoeles in tiie tuorkspaoe* extern 
decrees of fmodm are to bo proeitod* 

!lhere ere intoetriei epplieatione totoy ttert Itoi 
Mnipiiliitors liaw rare than six degrees of ffeedott* Wor 
»mmplo§ tho elephant trunk type memipoliitorM are need for 
lesrayi^liitliir aotorabiles* Freefu^y* the robot® aft pro** 
fteisned with leatottiropTb teaebinr progreMlngf Atre the 
nanipylalor is goMed tbrongh varioiii stages and at eaeli 
sti^e the doint positions are stored* 

It is natorally desiiabl# Hint girm the oMnwles 
in tbs tonispiieef asn^iilalor eonflgnMtioii ste%iM be 
obtaliiable tkrmigb sraie inrme soltitioiis* IMe nay beoMie 




2 


@s8«Rtia3. th@ iiariipij^ator ia vmd in u 

imviroMaefit* It is emf to a®« ^itit tfe® ii'imnm mlvAlmM 
t&r aairilpiul.«tor» ia esrtraasly cori^licattii toccius®, owt 
©f iiifinit# aoluttoris (ayisiafc,. to r«l4sidai'i,cieo)| only 
ttoa® aoltiticms mm wild ulsicli mmM tSi® collisicufl fortwam 
tii« 0 b 8 tael«s and tli® Mtnipulator* TkmB tiw inwrat solution 
n@c#SEitut«d til® iKxIflUieif, of wmrkisptMm Ci*@« iloncriptioii of 
obatuclaa in th« wrUiilMO®) to^athor with tti® nocWUng, of 
As stot#il 9&e%l^r§ i411 bo mra tboa on® 

nanipiilator oonflruimtioii for #«oli mtl^affwtor '^sition# 

Ttm til® iniwrs® solution proeo^ir® am b® roiirbly utatod 
to bav® two psrtat I'iwn •n£l«®ff®etor poaitloiit first 
find til® s«f® ^mfigm^'tiona aticl tbm oboos® on® sueb saf® 
iMlutioii* lioat of tb® olgoritliiMi tnokl® both tb® parts 
simltaiMOuiily* 

i «2 mvm’4 OF ' pmvmm mma 

Tim app-mmh to tSiis problos by prsviotMi iptboavt bas 
e®iit«r®4 around ai^soifyiiig a distmis® funotloii tbnt tmlss 
iiiv»rs®ly wltb tb® proadnity of may linli ultb m bbutaol® iti 
qpa®®* JUMiff and doni (197$) (ij pfopoatd an tbat 

giM an inoriiiantal oonfiguration of tb® nanlptll^r for «asb 
immmmwi and^ffbotor positicm* lb® "litfltnwM of *«®b 
obstatl® on Hi® llnlis ima iMnaurtd in t«rM of Hit itOLAtitt 
diatiii®® balMNiKi Hsan* XtuKtib* 19^ Ct] bastd bid ooluttei 
on lb® "aniftoial potantisl fi®ld'' ocsiifpt* fid this ipproiiobf 
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eollisicin avoiiliittcc# traditionally coiisM«;od a lilgi: Xmml 
plmmim- prohlmif was mflmtimly dtatrilmted bw^mm 
4iff&rmt levels oi control# pHtlosopIsy oi tia© artifi** 
cial potential field approach tmt be mhem^tieGlly described 
as follows* Hi® sManipulator i»vo» in a field of farc€?a« 11» 
position to l>e riiiched is m attraetiw i>ol® for ttt# ©ni** 
effector# and obstiCles mre repdisiw surf ace.-! for the 
iwdpuliKtor parts* The wmlptilator obstti^l# awDldnct problem 
was foimilated in terns of eollisioii of linka# 

rotlier than doints# Linlf collision wroldanc® ww acliieted 
If' eontjUiuously controlling the lUah*® closest point 'to toe 
obstacles* Kfrcarielcl and Vuicobratovic (1984) (51 det^eped 
m alforlthn bueed on pmtfommme indices in tl'io spme of 
Joint eoorddtoates wi ejctemal coordinates* idilch prf’Vent 
any of the Mnipulator linha from entartng a forbidden aoiie 
aroijiid the obstacle* Hie perfoiwnc# Indes was teaed on 
penalising; notion of tooae Joints idiioh influeneo to® notion 
of to» cloeeet point of a linii to m obstoole* !lieiej®wel(i 
wd moils* (1983) C^J eonaid«r«t toe obstacle iKwidwic® in 
toe fsmtext of a specified end^effector tiejectoff tdtli e 
specified velocity* Hse &ppmmb wm to Mefiiliie the 
reared Joint aiicl® rates for the aaniptilator t»ier toe 
oontonitote of iniltiple goaia* Hie prisMary goal unit deecrlbed 
toe specified end-effector tfejectory and aeootitery ioale 
deecrlbed toe obstacle stoidmee esd.tefls* Hit prliMry goal : 
end tos seeonte^tr goal were detoidbed by toe 





ob8t»c3.«»« fkm wmtm ns nmmm polygcsutl 

lM»4&#a sr IIS s iBiim &i poXyfofisl siiiipss* %iis@ mm mlthmr 
rsprsiwats^ mm m imJUm mi grnmrmiimm^ mmmm iSj nr s 
Mwisrsiint esiliiOisr f^ftsasatstiim m£ mmiigm'Mm ipsos 
ciifiAt C?J i*)ipsfts4 m ipses mimm wmsA imr m 

Mbils BttvIfstiofMl. mbo%« %iiS« i«ss st tiN» 

iMi&si 3.) 1^ tspsloiiaiiCi Isysl i^mmm mm m^&» 
emmsebsftt art mms la m mmmmtivity sad 





2 ) g#omi^Fl€ t«riijlbel]i assigns diwfi3ioii& to the 

&i thm cof«®ctiirity gi^h : niitii to tli«? mmmtiom 
m*i tmwyStikirf MmmsAmm to 

In til® mv£tik»r*n of tii® «^Ksce 

sentotioti sch«w to ©olv# for th® robot configiiiWitlon in 
til® pr»a®oc® of obstiscloa, li®« iwit b«t« #erllitr att«!^t#ci# 

1.3 *; 0 K or pfUSOT mmt 

In gmmt^ th® wailHtffootor n®i^ oxdy tua of 

froodosi for plmam ®oti©n coii®id«rlnc: orlootatioii of 

lost link) in a muksp^m fr®# of obotaclos* If obstatl*® 

&rm to b® atoM«4» sots® •xtra links art n««^4 for obatael® 
inroiditic® wanooiiifor* This aork do®® not i!®®! k’itb dMovttiiw 
tion of optinuo rtteslanoi®® r®qpir®4 for a partioiiilsr task* 

It is asstsiotl tiut tm #xtra links or® piwidsd in -tb® niini«» 
puliKtor* IMs wmtk ocn»ifitnit®s on flndliii' sofo Mitlpilliitor 
configiiMttiois for a girsii position* Xn soihi «as®s» 

it smir b® ti®o«8ssrf to rotrsot Hi® Msipniatort etrmsmmt 
m obstoel® if^ rsasit th® iissiiwi «ii4»®ffiietor poiitiMi* Wm 
m patb planning for «ii<l>*off«etor is n««Mi* this iiiofk also 
propoaas algoflHMs ftn* soob paHi plunninc* 

I Som of tb® d^sailviiitsg®® mmetmt»r94 in otutiar 

■ ■ ■ ■ ■ 

i)/ Um of iistano® Inbarontlf XmSa. 'to optJniai®^ 

^hm liilah hm to b® don® for oaoli ®o^ff«otor poaftiM* In 


©aw a tlslB may l«ad to oscillatiowa aromd til# typtimm 
eoitfl^Witlon «Ki a^sfMt^priat© |]0L«odiiic^ of lK«»i 5 «fi#©ua 
®oliitiofis ra«y Is# r#qMir#d [4j* Mowover, ctosivw^tti©# to m 
loOdl nliiieo is likoly and hm to k# ©ar#f)illy 

it) i^iMR th« oijmbsr of f%tei4oi»ci#s ax# tia# eoo|>y>» 

tatioR tiia# is Ilkuly to iii©r«os#t b^siis# tk# fomilotioo 
is «kipwidNsit 0X1 tk# nyai>«r of links* 

lii) tebU® tamckii* tm md^mfXmt&r tmjsetoiyt a ©ollisiM 
aay bsoomr inoiOiKit* In suck eaws« a now ©oxiflgyymtioix 
sKQf tM x#q^f#d t%> c*©e#8s tk# sM# poixst* Findln:, a n«w 
e^figtopotioxi Hid ealetxlstlxig th« s4iiticm.sl #o4N»ff«ctor 
patli r#^ir«4 to ©iwage tk# pmmmt ©oiifigtax^tioo tea teoiai 
«iitix#ly xi#gl#©t«d so Snstm 

%• pmmmt iiork piroposss a ibraulsticMi yhilisk la 
intefssiKiont of th» ntimNor of lites* this soten# moiAd tlitxs 
te a^temtsfoous stisii r#^hiiiteti©i«j mm lsx%«» lEctoiao# fmactiotis 
tmm mt tmm ixsoS sod ofsphiisia tes Isomi laid M a food topolo* 
gioal mpmmmtmtim of itfox^spa©# tesini; obstaelMf io tk« 
fnm of a diroetod gpe^di* A aoaroh throogii thit iir«i|>k yl«lds 
a roixgli path idsioh is thsrt miSmd to a aisootli (wrtit* This ^ 
©iir#t is usod to fixid out th« oanliHiiator A : 

sotMi# has hsso dav!^^#d to rsprxoisiit ^lo statai of tlis 
MMilpiilalor «dth s#^«et to tim otetaolss aioind it» IMs 
setimt is fofwwl to te wry usofixl 'idisxt a gitao 
poiitioii mmm ioiMSoassil^a* tteo tlia tmk is thtMlbldx 


f 


a mma m£® configuration of the manipiilator io to be ili»teiwlned» 
a ti^eetofy for th# wd-effector is to b® pl«io4 sucti ttiat 
the laonipiilator may nttwwct fioo th® old cofifi43i«ttl©» .anci 
roiiCli th® nm ont» finally at #wch increaiaitsd, liosition 
of i«ad-®ff«ctor th# asijlpij0.ator is to h® tn a oaf© ©onf^ura*** 
tion* 

'ill# Btsmmey of th® eppmmh of this mrk i® presented 
in flowchart of Pig* 1*1* 
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C::ttApifEa IX 


i-oamiuiriQi'j of tm 

2*1 BifHOtWCTlDllJ 

This thurntw ilvtailtd il«@eriptio 00 of at^fu in 

tfw foraiilatioii of nlgorittiB«« attstad #wli©r in Sec* 1*3 
the aanipuOlator tea beon oodellad m a ioootli ^’ae»2»2 

f iwa a genaml daacription of tb« pioblaa and l^i# itimtagy 
uaad to ®o4«l tiia Morlc^oca* thlm nootion alao doaoriboa 
liow tb« nxmth curve ia ua#d to ciCLcialote tba Minipiilotor 
eonftpiration* Hit mtmxk r^reaentatioii of *^0 iiieii%i|ioc« 
in tiif> form of a girapli ia U|» in 3«€* 2*5* lining Urn 

iiiow nttiiorlct m awotn curve idilOh ovoida oil obivt^'^^lta it 
culculottd* Hiit lios been 4otail«4 in Sec* 2*4* IfJOiillyt 
in 2«!!lt pttli i^mmiiir of lilt tiKl*>«ff*etoi* In ow of 
oolliaiini ttoa bttn considtrtil* 

2«2 cnouL sRscnmxoii or wimiM aiio mmuum w& 
mm»^m 

itiort fomiil profientatioii of ttio oiM|ili^ i^bodf m 
iteteribt In tiiia ototiorit ’^srn^h toot otmuiltot llio ob^jMtivea 
tilt ^ipfomh 'to tomuH&to tbm algoritlwi end mam ttroixioioir 
natd in tbit voific* ^ ^ ^ 




In th® pm&mt witU tl3i« Bwiipulifttor md mxkwpmrn 1® 
assuncrS to l»« plmmr^ tl)® obataclts mm to %»« 

oireiHor* ll.r«t »t«p in tli« solution Is to fin4 ^ Ctli«r« 
eoul4 tm wmf) mwm trm 0 to I tkiml4$xw< oU. tte phsismlm 
and tlia aooond stop ia to lit tli# links to oppmidast® tliis 
ewemrn I3st«fig|iiiiig a marva ftm 0 to £ is tks sM^r part of 
tlie tmrk and is dsalt witii in all foUowIng saoticms stsps* 
Hsra, m daseribs a proos^iiw to obtain link positions 
imd orisfitationsvdiieli i^proxinatal^ fit a iitm ot4rft of Imgtli 
miuai to tba am of l^iig'j^ai of all links* 

Ilio foUoainK oonvontioft is foUoiisd to laMl tbs link# 
sod ^s Joints* tim first link toimfds tbs bast 0 iilll bs 
foforrsd to as tbs 'first” link* and tiiat towartto' i&m stid* 
offsetor i411 bo oallsd tbs "last" liuli* lail Uift mm^mr 


REFERENCE UNE 



FIG 2.1 PROBLEM STATEMENT 


OOIMT 5 



FIS 2.2 -. BEZIER CURVES TO JOINT COORDINATES 
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wiil, bf n, ¥li®r© n •» total titmimr of Tfe<? llalt 

lidll b* repwswtod by a* is tb€ Itiog’tSi of t^w 1*^^' liiik* 

iim 

7m Joint b®tw@€n link i &md link i-1 will l» OvOl#! tb@ 1*^" 
Joitit* Tbti» th® Joint is tl» Joint -wtoicli eoi'»i«Jta lini'; 

(fi-l) find link n* Joint tniib^r 1 is tii® Joint at the bae® &. 

fh# Joint eooMinats inplios tb« relative rsn^^l® 
mtmm tb® two lixics coiinsotsd by titis Joint* 3o* Joint 
coortiiiioto #1 it till*" «nile betwec^n link oM 

link i« Joint coox^tottt# 1 is noosiirod irm ame mtarema 
line fisetd nt tli# bass 0* isosido® tbs Joint eoordlxiixtost it 
is also nmamary to tpscify to® eoordinntoa of too Joints# 
idileh is til# 3s*y coordinate of toe Joint looation in the 
plane of sioveaent# wito re#peot to any ooordimt# syalen 
ettaobed to it* Joint i wHl hmm toe coordinate# 
llie coordinates of toe end-effector positioxi S will be y^) , 
isid those of toe baa® Q »dll b® y^)« 

tm this aeetion we asmiie toot a enree tiem toe end- 
effector position K to toe base 0 baa beiMi gieen to iei« ^e 
mrm is a ieatier curve vliicb bae been imereted by ffiitably 
ebooein^ eene Beeler ^Ints in toe woriiepatt* (for toaoriptitoi 
of Beeier curve* e#e %peedix Z)« Die detaili of toe 

Beeier Itiinte axe elitoorated later in Bectlcni 

toe xeetiltiiif: curve is of a definite lengto uMcb is toe 
mm of all toe link len^tbs* tons 






m 


-Iti® 14«&1 point is eeloulatod m iollow# I'irst 
wo hwo to fi»4 out til® of th® p«irws®t©r v iiiUih 

corrosponds to poliit A pawwotor V| in firct calciilat®4 

aSi 


V. 


^•3, 


(2^2) 


pmsmt«i»r U|^ is i»w ©btfiijnsd fiw th® fomula 

u« 9 » sin Wj ^2^2 

■A m 

%ii«r® -} r®pr«s«its th* smscJteuB miasur® of tlio corx'oetiOE factor 
mmmmmrf to Oiiieiilat® txmt Tli® iptieinii of the i'«ad«r 
Points is utilised to olitsin j » l 4 it th# orclir of a osrtain 
B«si«r cunr® i»« Z* Tkim there are f # 1 Jiesitr poiota* let 
tile shortest distance lietween the and th© Besier 

Point iM* 1 |^* Thin m assoc iste a paraaetm* 'tn^ with ©cieh hosier 
Point such that 

It 

X 1 ^ 

lw»; ew 

t 1 . 

d-d ^ 

Mow ^ is calouilsted as« 

^ • P* imM if- «^il# h • 1, jr-I i2mM) 

tiiere i is a scslifii faster* A wsiut of # w a«f ^hts hsifi 
isisid to gits i^ood mstilts in the ipfilsiMaitatiixt* 
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riov,, giwi ttm poiat is isulC'iiaa'tettd om tti® 

ciarw Ijy asiisr* 

\ m i 



that th® palat H. Itmm t© li# cai tfet' »tir«i|’:tit litieo 

■*- 

^ \* *^™***' mppmmit$j(m trm th® m^®Simtxsr 

ttiil«« '-%• prmtukkm will he adopted ooIf Ihr i > 2 sime the 
position t»lkm hf daint 2 eon he oaleulated direoMy* 

P < X < tXm coordinate* of 1^ doliit are 






fm * m (j^ 
^i^ ^i 


C2*4h) 


^•re B is the slope of the line 



(?.*«) 


It mf m happen that the Joint limltstiiMti of the 
Joint is rmjtefrnd %heii Hj^ is heing eslotilstod 2«5)* 

In that mm ^ link (iel) is put to the li?sltli^:j positiM 



FIG 2.3 ; JOINT CALCULATION USING 

BEZIER CURVES 



FIG 2.4; DEFINING RAY AND END-EFFEClOR 

natural state number 
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md a n#w poaitioa ^ la ©alculatad* #ar2i®r» tto» 

icMBiitlofi ©1 Joint 2 Cl*#* e«i h& £mmd l>y ©«a.- 

eylaticn and It aim talc# only tttio poaitloiis* 4:i© on# 
is nmrmr to '>^ is talc#© as th# position 

Bafosw m pvm'mmt th« iaod«llii]|| oX niorlciiliiiit# and tti# 
Mfiipmator dsseription urith rai^iNet to itf wei is 

tak«fi up to ISrially illustii^t# tb# idsa baliifid tbt laodtlling* 

In th« pit>€#ssii til# taras us«d in tihit pm&mt mz% aft also 
dtfintd* 

fiir« ?*4 thro# otostsolts idl of wMofe art ciroylar* 

lilt bast of tn# iiiniptilator is at €»«. Xluft emtm of i»©li 
otataolt is Jointd to tilt bast o and tli# lint dCi) will bt 
oiilltd a ray* tlms tlit i"^ ray is th« my fenjnd ty JoiMni Q 
to tilt eintm of obstacl* i» obstaelts mn iaaliEMnid 
dapmdli^ on tlit anglt of its ray« 'Hit obstaoltg art ordtrtd 
In clmkwlm mmnm* Ctoing ©loclMiflMi from tbs my to tho 

vmy^ ttm wnt Imtmm ttei# i« tmiad liMi nont* 

1!lia antim aone fro® ray 5 to ray It ioing trill bt 

tha aont X« Xf tba «cidk«rf@ctor £ lits In tim mm# tlioii 
m say tbat idm "tndkaffaotor natural stata ' ia i# In l‘ii« at4» 
tba ' «i}ii*aff actor natyral stata is | sinoa it lias In wmm 3* 

^ adit Ci il is tba lino Joinliii ^ i^ soul ^ 
obtttaolas* ^lasa adeaa art nduays maibarad m Hiat i < i# flms 
if tte' mf^apmm hm n olmtiMslaSi tbin adgas mm fOsailAa* 



It is liitw tiMt lill lilMi ar# jaot rieci^*d t© 

tli« vorkipaG#* Fig* 2 *!> ate ¥8 ct %a 0 fli£|>ai$©' witF^ tim obutaclM* 
til# wQxkspmii is siodill#*! toy s«li©tlJ 3 i„; •<%#s iJ'iicii * 01 ® tsorar* 
owrlapfiiiir siich tt»t all Us© obat^«l#c «iC6S3(^ 

P«8*#4* Brnsm 4toaiiPito#« tis# coi^lat# proceilur® to ototaiii 

m prsctltoal 8#t of pwfswst lot 

«a c<m«i«S»r th# trianrl©® Jitooiai to Fig# ?#5* Eocls triaijglo 
is idoatlfioi toy m ordoriNcS sot ijlc i« ^ and k n^rossist 

otostaol® mmher® mad i < 4 < k# ttoiis tlis 125# 23^ 

md 3k3 form m sot of aorj-ovorlil^piiig tflilO^'le8 
all otostaelos# #<$g«s i(i#l) am e^lod "outalds ©di’Oto''# 

Itms adgas 12 # 23# 3 M» aiid 43 rsprsMNSt tto« outsido odgas in 
Fig# 2«3# 

Hio ifiatural adga '' is tisat '' imtsids ®%t '' rfiioto coirta^ ' 

/f f/ 

ponds to tto* ^nd#«ff«ctor tiatuml stats « Umis if ttos Md» 
otimtor nattsMl stats " is i, timi ths ' natyml t«%# " %M:>uld 
tlNi Ci**l)Ci)# 

Ws also nssd a itossription of ttos iiMMkipt!d.citor» IMs is 
dons assieiiiiig a " prsssnt nanipiila'Ior atalw wmAmt " to it* 
pntsMt wanipiiliitor stats nuotosr is dsfinsd toy tabing up 
Imo sasss toslos# M sass X tlis nsnipiilatof* doM not intsrssot 
any oiitaids adgs as stoosn toy position B9 in fig* 2*5* Xtwi 
tils pxsssnt a»nl|nilatQr stats nm^r is ^s sn{|«>sffscitor 
na^iral stats nui^sr# Xtius for posititni In 1%# 2*3# tlis 
prsssnt Mnipuitttor stats ntntosr is 9 sinos it Uiso In aons % 

Im 'ttis astoni mm§ tits naiid^iilator intsxwiots mm or nort 
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FIG 2.5: DEFINING PRESENTMANIPULATOR 
STATE number AND OPERATIONAL AND 
NON -OPERATIONAL PATH 



©Utsicte Cat in ,p©aitloa jta f%* m »t» 

th® ©tttai4« Im iat«r»«ctii€ wiillc (j-o4»r, £rm 

h&» to If tis# aiSrt iCi^l) i» tMo tliaii 

th# pWNMmt mmipMlAt»r «tat« aijiiiljar is (i-fl)* iliw In 
“i-lf * ?*5 th® pm«mt aanipiilater «tat® raaab^r for tiosition 

tf 

i® 4, stec® tilt oatsltl® 34 ia b#ii|f.T iiitercw;te<l 

first* if wt trawsf along th® eaniptilator fmi U to iv* *ls» 
outadti® imI| 5 # isfciicli 4<rttiinlJi«a the preemt aiinipudotor state 
nuBber is c^»Il#4 tiwi ‘ present first edfe® and io natd lattr 
to g«t«mt« tbe fiiftrt lt«i«tiw conffguratiori of tls« iwinipn^ 
lator* 

^9 will nov diiicuss jssnetliing al»out patb plmmlnf lUid 
introdne# tm mw terms* '' ^i^mitioMtl patt" and '' 
pa^ \ %ti«' apttfmticiiiid path will he deseribeci by the umr 
duriof, Met the aetwal operation to he peribrmd the robot 
wHI he done* the ix»r>operatioiial path will lead to idle tiase* 
hut* whioh in mm eiieea will tie easential to atpid a oolliilon 
or to reach a remote point in the path« egaifi to 

Fii;* 2*5 let m way that an e^ieratioii hae to he done hf traok* 
im points m 1h« path shonti by thue to end 

Ij to fow the " opef«4tional pmth "* 

fhe isieii^ulator oonfiguration at ia in rig«2*5* 

For thie poeition* the present mani^iilator etate ie 2 end idie 
m§>* 0 timt&r mtuM state is also 2* Ths md«eff eater new 
moeee to peeitioii neintaininr ^th# present Mnlpuimor state# 
%tie iMSif&giiinticsi et ehoisi wi*^ the dotted line has #ie 
prssmt aanlpmiiter sMe nu^er equel to 3# ^the end** 



mSimrnr inomz from poidtloii Eg t© to a^ilpuliit^r 

grijat® mmbm 2, it foiAa with olistiicl® 2* 'Hi® d‘Ij'-/4Q«8 mXvt^ 
tl©n thm is 1» taat® fr©a Eg, n^roach ©bstiicic 2.» r«t2*.a«t 
hiicK aft«F circwanwittog ohstacl® 2 ogMiii 2* 

It W0I114 thus tak# the coiifigui'iitiaa ehewo hy tfie 4ottifd IJto® 
to Flff, 2m2 md mvHd haw the pwseat swrilpwiwtar state 
nmbmt equal t© 3* Fwm Eg, the aaulpylistor bmi mu iceew 

point Ej to present waaipulaiBor etat® nvmhmr 3* *1» e©»pl«t# 

path then i» to Eg, Eg to Eg md Eg to ill© path tfxm 

Eg to Eg drcuawfittofr ofestacl® 3 is ©ailed toa " noiMp #Wttl«Msl 
path 'and this act to called m ©ham];® ", 

2^2 A 

the iimt at^ to to# aolutton procedufe retiree m 
aultiifele path for the Bezier Curve of a giwn lai^to iwm to* 
tjMiee 0 to the i«idH*ffector 1* Ftodtoi; Betier potota to toe 
workapto# ahould mt reqtiirt tie* conetMitoi; toiptitottoiia 
Oecauee thie eearch haa to b# done r^eatedly for toch poato 
tion of toe end^effector* A eyetiMiiti© repreMUtotlon of the 
eo'i^OP*®® ohatoolea ia deaerihed to the follottol! pwm^ 
^;r-]to«i tliitoirh totoh toe Bezier i^tota are detomtoed quite 
efftoiently. 

the ohatoelea which are pmmmt to the wortopaee yield 
few rtiriM* which are free iaid which wuld he poaaihle pm^mm 
ler toe ieaier Cmnni* Zt ia our ato hm to npreaent the 
^aee aitiuiid toe ohatadea in atich a wy that «ielt regteia ©ould 
he eaeily i^otified mnA followed tUl the arotmd o ia 


n 


for it to #j 2 Cloa© iiortsiag 

apa©# «i»ti iivid# it into j»4^1#r TOiM*wrl«|):pii\;; 
iMtw in thio eliaptart ^ ^ill »t a proc^^m to ttftalllali 

ii It ^ 

pointers 'biBtmm ad^oeisfiEt «ee®«Bts* A £m»ibM ;r»atfe eoulii, . 
be t&md bjr atarttei? Irm tb# mpmmt mmmst to It* 

Ibllowii^: conisettative polnirnns* 



bowMS^ry nt tim polygo©* ttiua^ oi»r rtquirMieiit is mtiiifieil 
if the bouf^rr i© I3432* Wla ia vipresmteit in Fig* 2«€b« 
nie xvsultiiig polygon la no aoro eonmix and m OaU it tb« 
otitof* poiyiofi to diatingoiiih it f joa a stidetlf eontoae 
Winona 
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FIG 2.6 CL •. CONVEX POLYGON FIG 2.6 b: OUTER POLYG ON 



FIG 2.7 a ; OUTER POLYGON ENCLOSING 
THE OBSTACLES 



A point oa tl’.ff cir«i«aif«rfnc» ©I tMa clixlt' i-j tw 
eSioaw te .'jct as a point for all ofeotado:';)* *llio mcX^Sf 

rioaaurrd froa ooae r*f«r#acr# lin® (lira 0® to 56-a® r&tA') ®t 
the first vlewihp point is ch©»«i vill be ctOJod the 
"startlnf!’ vlewlni' anirle''* Thm obstacles are ¥lo*,ie:1 from 
the present vlwlnii: point and. the nearest ©batt»i:.-lo is stored 
in n list, say a, 1£tie viewli^', point ie chimc^ed mvi»ci 
alorig the ciroisKference of the circle In any one tlirectlon# 

'Jiho viewing angle 1» IncrMented at Sfcwh step toy^ 

<sB m ( 2 , 6 ) 

%ds«re n « tnirber of obatiicles* Hie neareat obstacle is again 
found md wppmdmd to ttie list if the last mtry m» diffdrtnt. 
Fig, 2*7b showi the list ^ obtained fitm the situtildoii sioim 
jbi Fli-:, 2«7a, with Hie staarting viewing Mile equal to 0®« 

fbiving obtmifieil the outer poly£«m« tte .next jtep is 
to segiieiit Hiis into soaller parts. Me hove ehaoen to breah 
it up into iionp^oiieil^ipHii trlai^les, ttm psooedure to do 
this is not unique end any suitable nethod nay be foUoved 
without affeetitii the results imoh, Bereiti three eoee® are 
idintified idsloh are di^ieted in Fig, 2«©af b, e« Fie* 2*6s 
shows the oaee %diere there are no obst«e<oles inside the polygon. 
In Hiis ease, sets of Hire# idiots are ehosen fM the liet a, 
in eequMee, and are thMi groieied together to Jtoifi titeiiles. 
Obviously, the trlaiclts would depend entirely on Hie lie! A, 
end in ease Hie Hpim^les prove to be uneaMefettory, they 




2 $ 


®ay m&ilf Is# cltoue:»i fey clmi%inff tile startli^-i 

A triw^'le which io toe flat or too at*efe maleno lias to l»* awi<3ed 
fe«eauM» it creates oafeiftooits eitwitioiiia wfiile ac*t£is^;: potot#r»* 

In the secoiwi ®a»#* as a&yQ>m lit Fif., 2*Bh i« on® 

obstacle inaide the poly^:©a# Ttsla gives rise to m ossrty 
tri«it|?l 0 » ass there are unique elewiits in the liot caad ii».'4sh 
is ofetatioeil by 4oinii%; pairs of adiaceot ideoeiitu in the list 
to the leftover element. 

Wmn the ims#er of obstacles^ %jhich are not coniteoted 
by the outer polyi^iioii edg«s« becoiM oore than cm$ the solixtions 
Ci.e* the f&rmtitm of nonpoeerlaijpinis triangle#) t^ad to be 
sore varied and any suitable procedure can be edg^ted* Cine 
mithod is to idwitiiy eU elements Cl*e« obdecta) not In the 
list and istm m polygon out of them* 1h« vertioe# of the 
inner polygon afe doined to tlie nearest vertioes of .the outer 
poln^on mieh that the edfes do asot intex^iect* Tim above 
procedure can be used recursively till aH the points ore 
eifesixsted* Fig* 2*io illustrates a siq^le oases ^aftre the 
left out obstacles are three* tso obstfiolos inside the 

outer polygon* the inner polygon reduces to a line* 

Sm3 i- T 

7.3.1 t atnbll«liliiB «<te« HonwwitivlteH 

Die neifit step in the solutioii procedure is to establish 
rough directioxis for a path fron £ to 0* Diis is not to be 
coafhsed id.tli Dm paDi asDially traced by the etwl^ffectoiv 
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• till® try la?’; iMd u path fmm E to 0^ \m mm nmlf 

looittos:'’ for positiona *--»<! @ri«ati»tlffla8 ni'-leh 1S» 'SMipolat^r 
lirto could tEtlie* llh* orlcatatltm of n lliili 1® tii© a«i:l® it 
fomn witli mBpmt to « fiswd il» (lirtctioaji 

Ifer m fiatli tmm fc to 0 art mt m follow* At 1310- e;sid«ll« of 
mch md p«*p®ttill«;yl4ar to itt an mrmw is eistotexiabod. 

It is ft#sir»d that 13'i® oricfttatiori of tfi© lliiK clo-wat to 
that iw^’C should bs clos® to idh® dl-rsction sot bf th# arrow* 

"ill# task of soorctiim^ a pattt woutld Im* ^sspmdS 6#1 if tht 
comarisctiwity of the «d|*#s are eatablialiM |M&litt»r»* 

The oetaorfe c#ui thus he nspmamtwd as a gwph <^ 0 # nodes 
are the edges foratoi tiie tritoofles* 

Before we go into m <limuision of ttie edge connecMylty 
graph p we ciescritse tti# mdm of calculatioii of the arrow coordi- 
nates* fleferrinti to Fii:« 2*9* eonaidor «n witti ttio 
ohstacles at the mad points A y^) and B vith 

radii f ^ md f rei^eetively« The oh^eotiw# io to talculate 
the coo'KUiiates of the head U Ciiy* fg) and tail of 

the arrow* The coordinates of Wm aid^«polnt E art obtained 
fran 

% «* 

H 



% 

where ^ and 


lopt Hie poitita at wiiia!i the oirolti at A and B 
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FIG 2.9: CALCULATION OF HEAD 

AND TAIL OF ARROW 



FIG 2.10c-. NONE POINTING OUTSIDE 






a u 


ciit th« llsi® m ia p®i^i4ici4«r to lino 
©btalisod til® poJUat of irit«ra®ctioii &f lino l-K#, vitl’i tli« 
pmrpmdlculm* at H f i® otetatnud ty its tott»aetii3®;; 

|M9int witls line ^s!^2 pofpefidl€wa,«ni 

at A atid B ‘to lliia AB* 


lOfma if* 


ttaaii 


mMft 


i^tli lMmm§ 


C% - 3r%) 

Cscg - a^) 






X wi 


t 


f f Hi® 'W aar mm I HO 1 


I Im 


3%. m Mm * 

^ N1 * m' 


\ ~ ’'a ” « ‘**2 ■■ V 


% 








C» ^ 


% • • I (3%! * %) 


C2*8) 






Ca.icni) 

(2«iai>) 


C2«iia) 

C2«3Ulti} 



Stoilaariy f c^tti is* ■ It tuma out ttet 

tills pmmAum is slepisr tfewn ©slculatini!: cmMU tancsats t® 

cir-l«s at A arid B and tiiso fladlf^ th# point of lattrsuctioo 
wilii tfe© p@rp«iicii€tilar at B* 

i’ij® choice of tiMfr dijniHstl^n of tlie ar»w aeods caroiul 
scmtiny# If th# obstaclsa in tim mrks^mm aro sucti that 
tlioo# can l»« 14i«li:w4 hf pfetyslcallf uaconnectid cifctilar 
objects t tiiwa th« htmd of th# ttiw%^ is almys n@i:U'«r to the 
bag# 0 tlian the tail« This rule mn thus help i£'i deeidlnc the 
direction of the mmm in this case* If iteme of the obstacles are 
contMcted ixi mch other idLth m wiH «Meh the MtiptilistDr caisiot 
cross* then that eilge ivill be called a barrier e%#« the tesic 
philosophy' iHiderlyiiii^ the choice of the hm^ md tail of the 
arrow in such eases* is that on following ’^e head froa the 
tail side* the leanipulator shoiHd not coUide with a barrier 
edge* In the pmmmt ioplwientatlofi* barrier #%ts tuim not 
be considered mtd if stich a ease arisea* the data ripresenting 
the networh has to be isodified »aiaially» 

^le nest Job is to establish the c^miectitity of the 
edges* fo do this* one triangle at a tine is MM* iM#i has 
all the arrow directions assigned* three cmais nay wise here ; 
ae shown in Fig* a^lOa* fo and «* Fig* 2*10a shemt a triangle 
in which only one pointer points outside the triwkgle while ; 

other two point inside* The wrtieea hmm been numbered 
as i* i4l and 3h this case the pointers are ** 

CiHi^) and Ci‘*l)Ci’»f} ** il)ii^2h At i^idiol « is read m 
'' points t©\ : 



ti» CMae 2 lAt'm tii« iwjyattw 
■polutinf' outward. '’Ihm, tk» is 

and Cmm 3# « sliowi ie i» 

m Ctts# uhm tlil* tmm of th« sonipiilotor is ja triaici« 

and nci |iioint#rs wotild bo #ttab2.isli«d lioro# l^orc* \;oyld he of 
«jou»«, otbor poAntors bslonrlii*’: to them EStv^rt: tliot potet to 
tl-ioso €4f«a. 

finally, an illnatwt®* tho of a 

mtmf^rn fl«* 2*llm sliow® a roglon with 9 obatw^ss# tri- 
aiifl#* that havss Men fonswl mm 129# 234, 349 cad 199* 3^ 

#dip®s that haw Mm idontiflod ar« 12, 24, 49, 29, 94# 13# 33 
md 19« Bi® «%® ooiaactiirity graph for. tills is stona 

Ijfi Fig. 2.11b. fracking any <«® «dis® i»d foUowtog polntars 
lioijlcl li^d to th® nawroat adg® to 0 ididoh is 19. 

2.4 cmMummi qf cuiWEi 

^• 4*1 

iiitor fbrning the network, we next ehmm mm points i 
ill the free epeoe tlircnigli iidiioh m Beeler Curve omi be paesed# I 
the curve not jpcwuliiig nrlth any obstacle* to ttiis we 

will coneentratt on how to choose besier Points how Idie | 

lenirfeh of the carve i® addtsted. ' 4 Beiiler owm need not ; 

neceesarily pass ttrotigh the ii^nt points, tons it is | 

saooto to natiare. *Sim Besier 'Pointes*^ chosen to be at the ! 
aicHmint of the e%es* toe restilttog curve Wm eodld ti^ 
to be pulled towards the centxv of the edge, ilbvIoilAir 
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FIG 2.11 a-. NETWORK OF 5 OBSTACLES 



FIS 2.1 1 b ; EDGE CONNECTIVITY GRAPH 


OF NETWOR K 



enrm ia mt t© pmm throweh ohvtml^-trm' 

aiid in €am tli« curw £o%£Lb witft mf obstaclet it is bfwtoii 

ftt tlif* foiilim*' •ilr® ^oiii«Ni nmotlilly at Hi© 

utliirr n^prox.iaat.ifisf’^ ftaictions lik® a Xit ©r m 

woiiAd tM? coi^5utt©tio©ol3L3f nor® Imvolwd sand altlkow?;!! 

timy am hmvm j^mrp tmm& which wniAsi too atrlJi£;,etit for 

TOdcllihi'' a I'olxit aanipulator* 


To fe^fin with, Itt 118 aaswic tliw,t wo nr® ijiviB m 


mfpcmr pomtioin fc mm a path for t»© aimi^mator 

tma 'to ho found* mm mXm giiam th« " pmmmt omiipi^ator 
stal^ mad hmm^ tiia ' pmmtat fXr&t " as ilofinoi «arXi«r 
$Mi SKition 2m2tt3» 21 th« atat® of th« Muilpiflator is i# then 
the cure® hoe to pass timnigh eilge wtiloli is tlie preMit 

first eili® only if i mad © are not on the mmm sMe of this 
edge* Fig* 2*I2a itesa a woi%i[|;i«ke# with ^ olmtaeliii md the 
i»d»effe©'tor poeition is deelgtiated by £• Oi# pmmxt »anipi>* 




FIG 2.12a-. FOLLOWING POINTERS FRONA E TO O 
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FIG 2.12 b-. LIST REPRESENTATION WITH MARKER 





A P’®EiC'r t.wrw is f»w t© ciilcul&ttcl wife!;! points, 

lint before do ao, w« ^ msyr® bofox^tMit! ttet tli« 
fssslc'i* cuTvi* *io©s not ioo2. snjr of th# otoa’t&.ioioa# <4 
stopl© irtflo is followfHi to aac#3Pt4*ln this condition* Wc ass%n 
jaorkora CdoacFlbod in tbc foilo^inf poml to swoo of fetjoac 
iK%«a yhmm a collialon ia 2.iis«i3f« «i break tli© cta'W 

Jteto two or »r# parts* eacii Jolxied «ootlily at tl» auapicioya 
looklni'' edix®. 

jftk#aJL|faJjig i®iafls#i^ is don® in tlj® ioilowim,; ws^y* A 3List 
actMrtt i.t.« |.r is startiid fro# tb® ba»® old® and clnK!k«d for visibi* 
llty of o grm •acis point of th® list# conai^idi^; tli® 
of I'te* ^*12, m find that 0 i® '®isifel® f*oe C «id S Imt not 
trm A. iSo w» mm down ld»« li*t on® attp* and put a aarkor 
at B. Thd® ia by a ti®k nark in l%* 2#12b* 1M@ ehmk*’ 

iW ia contintj»d tjp tl» list* ^ chockUt? fbr 

visibility of B, It ttiuti® out #>«t B is viaibl# bo*®! frott A 
and 1# So tf 5 »r® Is only om in this caso* iiHich !»«s 

b®#fi put at #%« 13* ttsua tb® n«»b®r of B»*i®r mrmb tliat 
k©v# to b® »r» two* OB® frw« .point E to point B aiwt 

arjd^tr fron point B to poini ourws mm t& hb doiniNi 

awotbly at odf?® 13# 

So fat* tb® i fMfy tn of tb® ©urv® ba# not boon consi4or®i|# 

Cbir mqulmmmt would b® ®®ti«fi®d only wb«n m bwo a ourvt 
of a dafinit® lyy i jt . Bosiavart '®* ^ bavo a iwt of points ami 
if tbir# aro no cattHkOrs* tbiro a sinul® Boaiar nan bo 
obtainoC oast tbtro mm k mr^rmt Ck®l) Boaior ourwsvill 





3f 


m tfe« muiafoer ©f points lime hem t^km 

on till© bmzier txmte tor th® purpoa© of intciirutlxw^i* hum : 
cl»@€*ri 131 t© ij® 5 for ©11 tmmm* ration ©no m\i he das® ; 

hy uainr fomilis 






wl5®r# tti® ayfeicript J r#f®f*8 to tti® 


1^ woiilt! r«s>r*»«it th® lonirtti of tM j. Bosler cuiw* itot 
totwl lemgth of all eurws Is fownd by snwirii-: tb® l^n^tb 

of ill curves* Let tbt total calcolsted lemtli %»t l"« Ttmi 


, k 

L w i I4 

where k represents totiil ntnber of Besier 


(2»17) 


The prohltts of ad^ostihi the final lenc'th of the curve 
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jLij thm pTmiotm coitf J4»yrati€>isi« Ltt tti« 3r^c|ti|,3ft<l &i 

th^ lt»»t cyi*ir€ he md thm prrnsmt calcmated b# L^* 

Ahmi *1 i>otnt P la isb©a®ri o» th© llii® CX. (of* CY tliu t»m tmf 

b©) siicb tliQt W" w uih^ - u is wy ai^Atimyy eona^' 

t»t (immllf 0 < m < 2*^)t naiii is to b# d«t@3Paiia»i it«ratlv«ly# 
-b© tmf v-.lw© ©i‘ th# int^grul# say J * " la tm^-rntd i/lth tmd 
th« iftilui* of u i& by th® fomijds *» It* • 

K , _ r 

ciaculatiofi is r®p®at®4 till {h • S whor® ^ la soil® 

Biaiboi* n*pr®ei«itio|” th® alioiiabl® ®twr# 

%to tM>w all th® B®ssi«r Foiots hav® b®m iQvmA md th® 
mxt at^ is to coostmiet th® B«si®r coma nod fii^ th® 

Joint ccssrdiniat®® as dotallM fimrli^r Hi 2»2*2« But i#® 
hiiV® so far not ^®«ilt oith th® fint ^eoiit of tmnm irm 

positioii I to th® prsaont firot » IM® 
is tai.®ii yp now in th® foilovini ®ii^s«®tiofi« 

ii»t»na»mie c<igTO ...fiMii. 

hay® «arli#r i«f ln®4 tii® torn " pr®Miit f irtl ®%« in 
a®c* 2*2*3« ^® pmmnt first ®%® is always an outsiil® ®<li® 
ati4 is coispitrtd with th® '' natural ®<Sg« * to asscsa licigth . 
of th® fiiwt swpMttt of th® B®si®r eurv® fron E to ite prisant 
fimt ®ic®tt Four ®ai®s »sr aris® as has boon slioiiin lattr* mad 
mth has h®«ri <teolt sspsra^ly* 

Htfor® FOine into an an^ysis of th® fm» ®as®it ^ 
llh® to mlfmlm a oaall point rseardine " atiit* «hMii® 

%imn\ ^s t®i» has Atmdy hmn mtmdmm In Sic* t#2#5* 



mil m glmR pm»mt ttdiil|>i;aati>r statt* 

c:aa^ H %|i« "aaHiwil «%« "col»cidta wltlt tli® "fiiwiititt 3rt»t 
liiiA £ bM 0 l$M m ^{»a£i^ &t tli« "fMiMat fl«»t 







RAY 2 


FIG 2.14 ; FIGURE SHOWING CONDITIONS 

OF A STATE CHANGE 




m 


Tiiis is isliow In Fig* 'in tfels mm th« 

presfnt firat la obviously th« ntjttirol Biili 

certain procantions bw# to b# taken if th# Ito of vision 
©1 tl*e alfi^|K>int of proiMMSt first fc 

frora K la ofe»troet»d# If tb« line ©f sight i» elowjr, tfeiwi 
th# point P is token ®s th® first faster point th® 
foll.oii^lh|!' eilgss or* foi»d by tiwsskiiig pointers at ^plaintii 
earlier In See, 2*3,1, If W collides with miy obstael® tt»n 
th® next 8t#p i* t® place th® Bssisr point at the tail of th# 
arrow of th# preemt first #%#, In fig, 2,13a tliia point is 
F, If EF still eollidss with obstassl# 2 or 3 Cao in I'ig* 2«13b), 
anoliior point G is ciws«n suoh that it lies on th# Msoctor of 
ray 2 and mf % tt# distiinc® (X« is acyinitaii so that it is 
th® mmimm of th# oiit«iiaoat point of th# two obstaclas 2 and 
3» i»#« wuc C&C2) * ? 2* this ioeoftis#* a 

eollisioin is now not possibl#* 

Caa# li t The " natural #dg«" ooincid#* with th® "pmmat first 
sdg# '* and E and 0 li« on mmm aid# of th# " pmamt fimt 
•d#r#", 

^Ms erne is d®piet«d in Fig, 2«1G ia th# ainplost 
of all oas#s, th# aotual first #%# whieh dooMti. th# first 
hmier point, is now not th# prssiait first •%« * The mtmH 
first mSe* ih dscidsd aocording to th# following proftsdnrs* 
li#f#rrlt)g to fig» 2M ^ point £ li#s insld# triwgl# 023* 
ih# prssant first #%# in this ©as# is 23* foUow 
pointars fnas 23 and r#^»€h «d^^a 13« How m ehmk liistlisr 
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FIG 2.16 FIGURE DEPICTING CASE II 



FIG 2.17 ■ FIGURE DEPICTING CASE III 



m 


point E liea insid* triarcl«» 01 lii oiii* easo * is teaid® 
trlaurl# 013# 3© -m follow anotls^r pototor oiad w»cli 

13* IKslnt Elios outaiite triMsi::;!# 013« t'» tli« first 

lioasior Point Is cs»8« on #dl|ro 15 md thm mst of procs- 
diirs woiad bo m to »oc* 2*3*1* «i« iiil:iu«loii its#r@ 

E lisa oiitsiit® tbs oiit«r polyiroii Ci*o* tooMt tbo 
c; 155 i» disciasaod to Ch^tor XV* 

'll 

■ f . , ■ ■ ■ ■ 

, y f , , , 

/f' jf 

y;as« III * tbs " naturisl ®<Sg ® " mui the "prtsent first #%# " 
da not 

This case is sIjowi to Fig* 2,17 @mi i« a alightly 
cmplm Bitmtim* itony epproschos arc possible hem md tit# 
iiei is to ciiTwitnt oil obstaol## that muy lit b#t%.>««n tli# 
iPitursl state Ci»«« 2) and pmmnt otat# Ci«o* 3)§ stud reach 
pmmnt first #dr# Ci*#* edfo 23) § ftoa the oulMr* oMt* 

Itie first attp is t» check lAetber my obstacle io obaftryistiiif: 
the line of stoht frwi E to the mid-potot of th# preawat first 
edt,e 23* If toe lint of sight is xre# then m o4fi tti«t edge 
ill the list vmd emtinm as to Case X« If the Itoo of sight 
is beifir obstrneted Cos to Fig* 2*17) then smi# edilitioniil 
Besier points hue# to bs chosen on the ra^ 2* *IMi point is 
r^rsssntsil as d in 1%* 2*17* His point H is ohoMn so that 
distsnes art is sipikl to distanoe oQ* i is the 

obstsols niaiisr* to our omss i » 2 siiios the obstoelo to be 
cinottssntsd is minbsr 2« His iMit point is f os dMCIited in 
Csss I sni fi^lf too piMit of tbs ps««w4sr» is m to 
the Stirlipr too ciusts, - : : , : . 

. "lilHil'fW ■ 'UPWrw ■ ■ ■■ ■ '^pwi^ppappliPfP 



tag£j[Vs no " prm@mt fir»t '’ emi bt iiccg~3j&d» 

TMs ©a»« ia dopictod in fli:, 2,ie* ’iMs oaa® hj-® b©cfi 
inciwdifd to aolv» ® robot eonflgwition without ccjiMld#rttig 
th« n#twrk at all. 111 © pw»«fit aifflipnlator stet© ni»b©r '' 
is 1 Irs this ofts©, my eurm^ of L fro® E to 0 io a 

v«O.M cholc® as long as it do#s not intorf ®rt mj 

obstacl©, -A cubic cnrw ha® feetfi choaton far tht pui^ios® 
md m two additional point# ar« rt^Ajirwtl, *lhosc polnta P 
««l P (as shown in f'%, 2,18b) ar® elio^ such tl»t OPfl 
for®# a r#ctaml«« »« start with m point P pl&md m l^iat 
OP is p#rp©n4icuiM* to OE wd P li»® mmy fvm th# c©«trt C- 
(a# In i'ig* 2* 7a) of all th« ©bataclas* ISi© centra 

hmm hmn shown as c in fi®, 2,llijt A eiibie 3a«ier curve 
is now passed frott £ to 0 and -it# Iwifth L ' is out by 

intsfration, %# ©alciiaatsd l^th i ' is now cooparad with 
tht raqwimd Imicth JU » th© coriw»t©d main# of IIP i« obtaiissd 
fey the foiwila * ®ol<i itaratloa is ccmtiiiiMKt 

till fL •• i' I 5^ s iditra s i® tha 'SlloiwWLa aiwr* 

2,5 PATH puwim. OF mD^^EwmemH m ca*; of coiasicwi 

iittvs atlrioady disenssad sonaihlng afeotil pett planning 
in Bsc* 2«2k5* w« liad definsd tarns " «^«r«»tloiisl path 



path ' would fee raiitiirsd wii«ii a oi^lisloii is iwliifisti iq^lyie^ 
that tta''sft#»sffsetor natural stats "and tlia 'prtimt iMnipiilator 
stats" arc diffw^titi or a point is too far away to fe# aocomtad 
in ths " prmmmt nan%iflator stats "* M mmh nmm§ a mm>* 



\ 


\ 

FIG 2-13 <x FIGURE DEPICTING CASE IV 



FIG 2.18b SOLVING CASE IV 



m 


©p®«itiojCJel path hrlng tai# "pmmnt ai-mipul-tor «tat® " 

to tl:# ' ©n,«i-«tf®ctor »atua»l »t«t« " th«* puiiit €» iw fe# 
safely aceessed* This action <l«serihes a pmee^to.;.’© tc £ixid 
out tiie " iioti-i3p«wtioisa'; path " to earn of colllsiori# 

a# “ pmmnt awnlpulator state mx^w ' m%il %b« "mxi- 

// 

effector wJtural state wissher solely decli® the m»*>»op#rstional 
path, Hef erring to Fig* 2#19 %» find that the «^erwtlornil 
path has h«i>iis definiid by stralgiit lliif® ^olntof^ points t 

SO 

and^on up to point It so tw^pens tinst at point 
the rucct point is no a» re sccsssihle to pr»s«t meiilpnlator 
state nijBihep 2. Ilie ileclalem to retract mid &0pmmh this, 
point E^"fixm A new dlreetton Is mw taiMn and the of 

obstacles to be clxtsiniiieiiteii is to bt found oiit* Suppose the 
" present nsiilpiiilator state nueber " Is p md the " oiKleef f ector 
natural state nijsber " of the n^ct end^effector position hJ is 

' m ' 

q* Hisfi the imaaber of obstacles to hm cirewtrwritod are fcf'pU 
*ilie obstaoles being elrcuneentsd elll have the mmbsrs 
p$ p^l* •«*» orl ^ !>)• ^ ^*19 the present fMoto 

puletor state nuirilier is 2 md tim endr-eJdfestor natoA state 
nM^er at positiofi •* 2 obstocles have 

to be cirC'Ufavefited* nai^ly obstacles 2 end 5* 

The path to be folloved in olioltoi around the c^staoles 
is f|ulte easy to find one# toe obstacles to be olituttvmited 
aie idontif led* Keeping a safe aargin t aroiund tot obitoales 
v;.^ .ipproocli toe first obstacle teiii«mtially fron toe pMsent 
toiHiffeotor positlcsi Sj^* tois gives rise to toe path 
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OPERATIONlAL PATH 

non-operational 

FIG 2.19; CIRCUMVENTING OBSTACLES 
IN CASE OF COLLISION 
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A cifoular path la «txt tli« a-ir**? y&tditisi 

till a path 3^,3^ to the fiext obstacle ia foi«4* 

'ihls pwcresa of tracisng a path ©loniT th® out#r p«tdt>li#rT ©f 
much ccnsecutlve ohstacl# la c<mt$mi@d till the last ebatiicl# 
is r#-iAch«»4 C3j Vmm ttitre a tamgafitial patli ^.■^ la 

foHowd to the a<s»t aad-aflecstor poaitloa of tlw op«3r»* 
tlon-il patl:i %t.lch 1» E^* I» this proeeas the lasnl** 

pulator itiite twaihor '' kmp& ohaa^lnfi, @*ch ttoo m ost>03 « my^ 
so that «lieri m ar# at the mm poaltiou E: J* -Sh® " pi*03<«t mmi'^ 

m 

polotor s'tat® miribar " Is oQiial t© th® " atats 

ff 

£f th« point is still not a®e«iai^« thin tht point is 
fondly inaecttssild® hmmmm of insoffieisnt linli loofths or 
limitations on ^olnt fotstimi. In aiioh a cam th® eas^ h® 
abortad or tii® nixt «ti4-®ff«etor poaitloiit is iBrlod* 



t nmm lu 


F0i#iuL/4SXi.^ Mm immmmmiM or 

3*2 IWTI'SG^JiCTXol^i 

In tht* pKwJtenis chapter a ai»tl»4 was dUlsaijWt^ to 
dy#t®rwiia# th« ^olnt coordiljsat#® of a rotot with fswtoSont 
links# Hi# involvos firstly rsproaotttlisc? ttt# wsAipoB® 

u« a ijatwDrk ytoat iw%« conisictivity 1# tatablistoed iii th® 
fom of a graph, Ifei® artwork is ttiw4 to find out a 
cuiw froB th# ®acl«#ff#ctor position E t© th» hmm 0, lli# 
at»p« of t^r C'ww is ehostn so that it mmi^m colliiJoa 
with tmf of til® ohstielos lying in th® SMa 

onrvii i$ now umd. to fital out th® doint coorctinat#® of tho 
rotiot, All thiroiai'.hf o^y plamr oaso® am takm idirrr 
thr ohartciolrs am ill oimular mad tho Mutiipula^r is imr to 
w»yr in this plaisr only* 

la this rhetor wo pro.pom to giro of tho iaploiion* 
tatiea dotail® »tsl tho algoritim doseriptloa, 

5*2 mviliS'llfAflOMi 

^.ii^r»sotit«ttlofi of tho niitwofk m^nipot on ofUftiont data 
iitruotttfo* Hi® data •truotum oatoblisli®® a fi!^atioii#i|p 
b®t«®«n th® sfddao^t «dg«s* meord aloo mafm tho aid* 
points of tho «di®st ^® eoordUiat«» of tli« hnMl and tail of tho 
wnmf attaoli®d to tho odgot m well as tho obite^t MEid»m 
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iie ut its In figurative^ fora it sia:i’ tm rw>prmm%te^ 

asi 



ii#od * xjiiMjsrjc«»rdl^ 

tail 1 x^«adjsrjs©»rdlaftt««| 

iiajttjwlgajz I pol»tar„t»j@d£;« 


Hia foxnatlofi ®f liiir »«t«»rk rw!|iiirta firstly Dsniiig m outtr 
polygois around all tba glvwn ot>«t»clra, Tfe® ototi«lt» mm mw 
salnetad witii ttira# in m §mm ^ poasliila aoimwlipptog 
trlmiXmm art idanUfiad* ttia Wilmas of th«i# triMglai 

ar# atmmm om at a tiaa and tiia ralatfoiialiip datuMn Vtm 




foraioi^ thm la dtm& by 

pn»c&taBm i&mjatmtmrki (3*2} 

foffls^0iit«r^l3rpwi| 

®{iii| 

Eftch of th»8w» nnbpmtim^mm mm ^acrib®d l» tl» follsswlii|.'' 

thesa aubaoetiona* 


3.2.1 


'ill® flrat procedur®, mswa-ly foi»^©ut®rjpolyi::ois Xmot^m 
first dmmvibim ^ clr©l« 1® -imouiBb to ©neloae all 

obatael®®* tb# obat»«il«a mm ttim ^Immd frao a pplut wbi«ih 
livs oa tb# aireynife’rtm® of this citvla and tJie naurtat 
obatixsl® atorai in stanli. 1%* vimam point is in 

ffinull ataps a^ng tti® ni£t(Uisf«r®»ei@ WKi thm premmm is toiiaiiintacl 
vilsmn on« rtvolution 1® fonplot® to 3««« 2«2«4)« In 


i^lgoritbnin fora it is tbus writtin as . 

prmMum inr®j&iit*ld«,js<ilyi©nCii<int«g«r|*f jr* army 11* .nj 

®f«lil}| (3*3) 


ijMtgin i 

mlAjK. •• <isa3i(34a3 t© »in3) ^ iiiiiCiClJ to 3El»3))/2f; 

■ lald^ I* Ci^(y£lJ to jrlnJ) ♦ isiiiC|ilJ to yCnJ))/2| 
radius •• mix C(«»» CfClJ t© 

Cm** C*ll3 t® s£«J)*«idiJi))| ; 

cllvuQMwfJiist.^ la Of 
roMi (stsrfcl«f:jingl#, iiittytetsrfa^ 
snt,?l«jlji©r«iii#nt i* 36i>/««ajtet#rt^ 
inr ii i« 1 to ai^int^mwils do 
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im ♦ mtliua « eo^ Cstaptiiic** 

awe:!# * ic IS 

tm iiiidjr ♦ ra^iiia x alR Caterfeiisp^ 

+ k IS ^;'le^ii!i0r«i«mt)i ; 
find n®w@at point froa irlw^)l 

if l4i»t^eli!w®iiit prooeat^olocwiftt ttifia 

clrc\iXm^lmt^mUi%0v *«» circular^ 
li®t^30iot«r 1* 1| 

olosa^nt tm pm^mt^ 

®adf point 

«mi| 



tovmd th# outoido polygoti guriwwttog oil 
©botaclo®, tti® noiit ottp i# t® dlvid® ttooiitaiil® 'pol3®oo into ^ 
m mt of ftofi»oir»rli^pli3« triongl#®* Hi® oimtrto of tfo® Qbst&icXm 
mm th® ir®rtlc®8 of th® triaogl®* th® ontor pol^^^a lAiois l»® 
b®®fi mrrnmX will h«lp oa to Idontijfy aon® of th® #%®t of ^®®« I 
triisivl®®* I 

Hi® «ts«t®gy adoptad for tor^mg trlM^Oi out $f 
0 tit®r polygon haa l»®®fi diacyaaod in 3«c« Hit pmmmt 

tmrb haa hoimmr not dtalt with any eat® ilitr® tiie nuAtr of 
l«ft out ohatiiol®® «xc®®d on®* But th®*® «t*a®s «£» taaily h» i 
inoorpofwtod by uait^ tl^ proo®€hini ntntictitd in %ct I 

Hi lOioritlaii® iom m mf tiUM .atat® : 
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pixscse-dur# f©rB^tri«!igl«s| 




bf»fin 

ctt®# CtotaljEiyialMrrjjf j)batti€les -* uireiAarJllst 

, poi&ter) ©f *“ 

Of begin 

repmt 

for ««©li elemnt bS tlit Hat 4o 

bagia 

takff tliwe elmmtB Srm- lUf 
and tom triosf 

pmomtJiiBt i«» riw ii»t| 
laitil pmaafitJlIst^a^OBptyi 

€il4f 

It bf|;'JLri 

rt^at 

tak® two naaMute twm list anci 
cottbijn® with alasitfit iwt JU-i liatf i 

®i^l 

btheni 'imgim 

w "W‘ W(Wf' %. wim 

*toia_liai«r ORf 

ai^sj 

mAt 

^9dt 


m# Imat 9tm tn MmiM$ th® aatiiojrtc ia ta iiiSfttify tlit 
®di'#a ®f til® %Timt^m§ ctiiooaiag tbm &m at a tia®* 21s® mtmw 
attt^ch®d t© th® ®%® is mxt Atmm of Us® 

i© @st'j^blisase4* Aa 4®aerii»®«i i® S«€« t«%lt WPm mmmm 
aria® aisil th«a® bmn <i®ait a«fS4ir®teiy to tti® fc^lawijng 
l>K»e®4wr®. 


£or e^ch triangle t 

' > . ' ' fcitt .'aim. 

Wm^mM 


C3*5' 


id®atiiy a2iji%«iLPf. 
tor ®%® i 


1 to max__triangles do 

rjwiittsS 

% 3 m 


5S 




if airoiAtecI <&t {%) is Inaidii 

tri.asaf'Xc (^} tli«*i 
s» t«io 

«Is# 

t«v fsiliiti : 

«tKi| 

f@r ««ch «<3g® i *«i 1 to 3 
bi^in 

If sirow^iimM® Cl) tJiei’i 
te«fln 

11 tlMt othstT t» iirmw»t w 
V ar® poliitlnc* outmlAe W>m 
h&gin 

(Miiittivib ii) I 

CpoljxtarJfe^®^® jr) I 
and 

#Xii0 

if only oiie oX tist oth«r two ; 
aiTOWfs as''@ pointlui^ 
outild# tli«ii 

IN9^IA 

«%0C4)*ls«»!tjtdfa It** : 
poteiar to tiiat #%# 
ymcMi tall Is iMldai 
a^#Cl)*ii©aitu9%f«^*** f 
mad riilf 

mlm 

Cl) *l»3Stjld4?« JLI « 

tiHi 

«ii4| liilf 

•i!^i 

«ad| 

•ndf 


tfm ustw&rk Is »cm usady* Hurlng tlia palutmi' will sat 

as a giAid«Miy Xroa tb* oiaid^foatKir pa^sltlM to tbs baim 0« lin^atsr 
lias tMNm 41soas8«d m far Is 4o«i« li#f«>rs iiMiittoii aturtii* 

In mm mi static ctstaelcst rwtiiof^ iKtll net #M@t md tli« 
iMMw nstweit wUl t« iiseit te flfti out tii« coiifipnKition for cacti •iicl*^ 
•ffCctor poaitldi* Ibis wiita.4 ttius fon {Nurt #1 ^ ]^ft««^i«cccftliii 
tlttc* Xa * il^eiattle tb® mt^amk idU elw®»i Isit now 



m 


would fe# fallowiwi to itio only 4lff^r«©» tl»t 

aay occur la iu iai« position of tho olistbclcs tfco looatlofi 
of t^i# falt^point and coordtoatos of tto arrows# 


3*3 Fii^dEic f';iy.iPULm»a cciifKiumfioia 


%lm n«ti«irt: yhiich feas Ijoeo I'snayated fey procodurt forei^ 
ii«ti«POi% is ustd to find out tht asnipulator ooafl| 2 u»tlois for 
mKsh sod sffsotor poi^tloii* thim is tiM ’fe:rok@» vp Into 
tiio wife-'tesits# 13ms oiwwpiyill ©ontroX of tbs toate i» dwoo by 

bsgiJi { Haiiii ProgrM J C 3*65 

rsi^t 

fliid^ii#3itJKi.jiO8iti0n| 
filid,jssnipiiaatorjBoi^^ 
until tim of t®sl£| 

^ a|PlS(|p iSimp * 

•ndf 


'Huiso tiM sub^tssks «r» dliieuisod in tho foXXoving 


Thm nmrt «iid<*«ff«otor positloii di^onds on ^kitbor it is 
trookitig no op«e*mtloiieil pstli or a mfaf^&p^mtimwA pol^* Tim 
doeisioa tbst s msiropor-tifMSl pstSi is to l»t tmmkuA is 
diaetissod X«t#r in AM^mlng ttet infomottoti 

olMiut tbs naturs of tlis path is slrsudy tomsit tli# outliniw for 
dstsminiiig tbs sml^ffsetor position ftri« 


nroos^kirs findnsiet^ll^msitln i 5 «?) 

w ■ _ Wlm wwilF 

iN^lin 

bi^in 

find His nssrt point by ilffod^t XiM 

Intifpc&ottaif 

Mid 

<dss 

Miiin 

©!miiis tils nffist point m> tbs imNs^spitimiX 

pal^ lAfOwly ooXotaotidi 


At tills stAif:## l#t m ««s»# that tii« eonditloaa foJMviiv 


a i»»*©p«if«tl0Mkl path Cdlscuaaiid in Sac, ' 2,5 i «pp ©atiafiad* 
%«■ niwtiar ©f ohateclas to ten eirciMWit «4 art l,w«®s* 
-li»n th© path b# culeijaatati ts^ 


pmcadur® find jaon^opiratJbMml nisthf C 5 *B) 

bagin : 

find th» ditfawmc® batwaatt pjpaawit i»anipwl.at»r 
stato and ®nd«*«f factor natujnal ctotoi 
dceidi upon th« nuiibar cf Qh&tmlm to be 

olmwtontodf 

find emmm polygon farmed by tlit« obatacl®® 
kaaping n mim nari^lfi ep 
tnica a path on this ewrw at mAl tetort^s 
tomrda th® bwn aichi On and 'thatn in 


midi 


an 'mmf$ 


3 . 3.2 .^iffifliltesaealtet* 


Ilia prasant and^affaetor poalttoti and ^tli« pm$mt 
atanipulator state %dll d«cid« tha imipulator confiftiirntte* 
Cmp^rlmn of tim prommt nanJ^Hilator atato and tlia ond^ffoctor 
nntiu!*®! otnta docldos idiioh of th® four oaaoa Memmmd in 
3oc* 2*4,2 will MLd, Th« diactiasioii in See, 2#4«2 i» nov 


proa«nt#d in mgeritonle £&m# ■ \ 

prooedwfo fii^jaiiriipniatorj»tiit#| (3M 

bofin 

eaao 

SEE nwtwaljitato «* pro»«fitj»»lptil»tor^8tatfr‘* 

of^ln 


mm 


tl is bffond the prmsmt£X}mtjtd$m 

imm ffi 

bogln 

find « pnto tfm to 
prsto«t^wi\j»%®f 


«li4| 


iE towards u tmsi 


l»«g;lll 

pr®a»iit, 


«ai| 


I;:® I* 

I'lMt OdrCI 
^iiXe XXm UE dSmmt 
tolsawect pr»s«iitjfir»l 
«%« do 

iKiSo Cpr©a«itjiig««f4@xt^ 


ondlt 


isuild^itTOk^of j^eo I 
aaa%n isarkerm to 

f iod^^liit jB©@ M jyimt#sf 


afUad nwa^r of ofeotaol#* to 0« J 

0fosaNi4| 

find a patk ll?oo E to \ 

“ i«i 





»«l«c Joiiito | 

flnd^oltitj!»oi:xiiriiit#»| 


•i»l| 


«orJk>t ^ <!«€id»4« 

fit o onrw of ofd«r wtliioiit 
©oiMiiaorfai; tt« iiotworki 
flI^^l«t jEkioMii»t#ot 

oi^t 

(Eaidcait#| 

oiieek^ollliKlon,;^^ 


i"lio ayoroutiaos umA In tli« etom pxmmim mm WLmussmd mw 
with m wb&rt 4osor%tion* 

%3A1 

m* •nd^ffiotor ^iwal »lat« is Just m iwrtNir 
m tl)* sidHiffsttor positloii* Tbis mMrismr is wMid lOr MifriS'iiiw 
^ilpiisss wli^ tils pmtmt Miiiptilator ststs m»t fiM to |i« - 


taktfi ^ thi! Curvi* is dccldt^ upon* Ii'i pli^ 0 i««iX t«i»a# - 

thin wiald r«^r#a««t tii« m»t m^tmenl mmm t© tfe# 


p@®itioft from btia# 0* Tii« proois&ifif for fiiidift;; <ifi«l«*#i’f«ctor 
natural stat# ia aa folloiiw, 

prO'Cadur# (5*10) 


baftifi 

tmtural stato tm i 0 | 

r*i^pf®at “* 


«sA| 


»«tiMral atat® im natural state If 
until and ®?f#ctor iffltl# > pFatoclejwairlo 


5*5*?*2 BMAliigML3tiffi||..M 


111# ataclit yiiiels eontaJUui ttui itidtsc of tiit tdg«o» ’ . I 

th# n>iJi;li path th« bast 0 wi iti4««^''fffetbr f * fliis | 

path is moothim^ later hf usiiiit i^eaier Cim^w staek is - ; 
build hf essentially folliiwini poJhitefii f rcm the 
fliMe, the pmsedwe ean be outllMid as 



do 


proee^re C3«ll) 

begjto 

steefejiop t*« li 

•lemnt i stash^topl tm present^ 
uliile presentjftdge* neist, 
iN^la 

staelc^top »*» staohjfepp ♦ 1| ! 

present jedfe nearest^tp base Cppe»#«tjt%e 4 

It pwenf edfe:::,ef«wEtjplf':#^2)l 
«lei®«it Fstee^ti^j «• 

- ePdf 

msA§ 


5«5»2*5 

lies>h«« art assi^ed to one or mm Ihnt mm ptesw^ 
la the mtmUt Ihie is done to breali the eurve $m& mm or «ors 
partst to muaw beiordband petsntielly diligifiiiis obstiel^^ 
m not eonide with the mmmm iam 3e©# Stm^lh tlm«|h 


tills «lo«ii not gmrmtm m collision frm patii, it ia ri«»rtl»lC8S 
umiul to aimld i^olculatiois to «©»# ©«»«s ^ mlliBiom 

la likely* 


I'larkora me put at ®o«# by eb«cklii4; ^sat tb# ^ 

linie of trm tli® start of the to til© la lr®#» 

ftie coo|>l®t® dNiscriptloii was given in .Stc* 2*4*1* la islgorltitoie 
fora wo ha-m 

pmem^Mm a#8iga^iia*%«r®J:©^#%®»f (3*12} 

begitt 

v£«wini’:jp©iat i« teas# coordltiatosi 
stackjpoln^r !*« stooEjIopi 
fepoat 

n^il® mlmmt [iitackjpoiat«rJ#ffll4 jpoiat Is 
visilil# trm vliwljjg oolat asd not mad at \ 

stock ^ 

atMCk jjoiiitor i** «ta©ii,^lnt»jr -If 
If not in^oXjfffeocii liiw 
begin 

ataekjjoltttcr i« ♦ If ^ - I 

assiga iiarkor at almcH Cf^k^^iatarit 
vi«»iim':,j>oliit im Ammit tatockj^ointorji 
mdf . i 

until endofataitki 


3«3*2»4 

Hi# atack almigwitli aaitera are mw used to atilaet 
Baaier Palate wliich will eetiOillsli tbe i^aier ounsid* Idiea a 
narlcer is pnteent at an e%e* the ctirw is braken up at 
•dp.e and nade to pee® tbfoturli tbe mtde^paixit of iliiit edge* Hit 
two cunwe mm Joined 00 tliat cimtlntii% ia wsi3M^i$m& at tba 
Joiainf point* Seleetiiii Beaier Points is 4i&m Ilf 




is« 

pvmamtjs. t»» ©ad 

pi*as«»tjjr tm «ii4j&i’f#ctarj)oiat^'f 

cihcMsn® tit# ippTOprlat® B#«i«r €iir»| 

Ciilcujaat# II for i^qyJLwtl potetf 

CWf f?iirw^a>i«fe«r#35*y)l 
frM Cpr#a#at_x* p«is«tjr) Join a atraiglit 

Ito# to Cis» y)f , , 

cat a Itisgth liak^I«iii:tln Ci) Ills# at(x*y)! 

pm&mtjK im m‘$ 
pwmmtjy y'l 

4oiiitjsoordlisat«iCl) i** difluiwio# la alop# of 

wBacfiat linMaf 

1 « i-***!! 
taitii 1 w 2t 

'^p^awwpflp lapipf ■w»!w « 

«ljluatJla#t^twQ'JlJUsl«a y0» pr«a»at x# pr-eisc-ri.tj', 

iaidt 


'%# wSmm pwmw^m iavoXvaa tiio proet^reoy isMtaly 

flndjpointj9aJ-¥iil4irjBasf^ aad a«3Jii»tJl®®t^twoiJll»A*, Ks#a# 

ar# now 4#iicrlli«4 In foXlowiii^#* 

rindlnr a point on a Bailor eiirv# la m aliid# mtm 

tis# eyrv# «n4 J^sl.#r Itiiiii# or# knoim* flitis w» hapo 

proc#4«r# find mint on B»ii#rCiiiw Cn*i«aXiOttrv#jttij«l»«rt 

int#g«ri mr x#y* rtsalSf C5*15) 

mr mx^iuts i lnti{g«rt ' 
b#i;lfi 

K ••» Of 

y l» 0| _ 

iiiiiy»liit# iM rtwb#rjBf ioi»¥#^iiiiife«rJi 

for i •• 1 to iw&ioolists do 
b«iln 

Ci«i ooiabinotloti (Mi^^iiitsi i)f 
eiw Cl«»ii)mi Cni«,^li»i-i)ji n im 1} 

XI* x#^alorjE>olnlyi! ii,oiifwj9iw^rJ 

^ M 0 M ilf 

ft* jr-^ilorjsolnlyr 


cndf 


#ai| 



m 


po»sibili'fci@3 l&lkit a4i3f rftsifitii tb® ©iie tsfeicii ia iMisaiwi* tti® 
previous Joint position will be pmi&Tved* A ^acriptlon 
iid.t!*! 0 tit the loi.:ic ®t»t®B«int8 Is now' giwn tjolow# 


pwmrnum iidJostJ^ast^twoJLjyifes C3«^»r»»x£^tyg#eat«|r#4*iS 

mt* 

wer a,K2i rosli 

tesfln « ... 

Kliw CC3i»*3|4)' * 111) I 


M) 


#ncl| 


th^ta im Axm ♦ C03IWV 
3^ i« +111 jt^COS CtbttaJ 
iw US ♦ si » SH* CtJNrtaj 


«itts tbls i3Mt ^ucrS^tlmoi liiKysMilptilatorjBonfJ^uf^ 
(aliroritliBi 5*9) nssds only ob#ckjo©lll®Jtoiijso.iio» to b« 
the rssultlii:: eocyripnmtion will siipfoxteattly tal;« tlit iteps 


tiiksn hy tliw B«sl«r eurvw* But eollislon BstwMKi tlw obstMlss 
and tin* eslciil<i)it«d link positiiHia arv still llksly* So tlia next 
stop is to thmk wii«tli»r a eollisioii 1® tilling ploea« wd it to# 
to inoorpomta mrvmitl’vm mm&yrem that tmi to dirrinata 
collisions* Hts nMct wydbt*->a@€tioii doala witk tkis pnolilsn* 


3*9.^«S .ajdlm 


fits pitioadliro eksclcjeollisioiyiasss is in ti« pmta* 
It first ohtcks out collision co®#® if anyc siid tlmii takas 
eorrsctiwa tsaamirts* 


pwicsdiirs find coHlslonjsaaasf (3*17) 

iitito 

tar i iw 1 to tMstar afjliiiks 4ot 

for J iw I to tuiSbsrjif obstwolM 

cliscic idiotlicr link PLJ CMUid#® with^ 

•nil 




}'j«ssler points followdMl n ntcsiiculutioii of tSie ^ofat cooisii- 
niit#ai, Th© proc©dyr» to isodlijf tho EumlpulotoF mnitmr&timi 


sail l»« vtittM m 


proc«4uiit Biodifyjwinfpiili*torjeoiifigii3pyti0ii| C5*16) 

b©gl.ii 

pyt » Barker »t 9^i» wiaicK Is coimicfctd to ttee 
fotjUaii'' obat^Acle «ici wfmm aidjtoiut is nearest} 
select B«ssl#rFoifit»f 
fli^Joljitjcoorsiiiiatefii 

end I 

*tie bI»ov» two prmwt&.um» are Mdsd t©g«tli®i* aad owablned in * 

sln^'ie pmcsNiiirt eliecte-collisiojwsaNiis, If tli# cwilif Icatlsin 
done by oodifyjBiiiipiilatorjcmfliuwtlois do«® mt gite m 
collision free path then a state chw^e is iMCCiiiswy* So the 
lainMspsratJbiml .path iioiild b# calculated and th# iiidncffcctOF 
will be directed to bov# o» this notr^opsimtioiis^ path* %• Bay 


now cfiuBsmtc thcwi stsps in tim foilcwim! prpctihirir* 



proesdiM check collision casssi 
bn^iii 

flai^^llisiotijtassst 

if 

bi^in 

Bodify stfii^iilator^eoii^^ 
find collisio^CiM»»| 
if collfsicHB^isjicewing 
iMii^in 

' findjooEnops»atlo«Bl^ 


m 


the €ti 1'* ebam.:®* th® 

Op#rU4^S,Oil Is ©W’3r|l 'lb® €3Ei£i“*®flf#©'tSOI?' will 3r®QI,ffl© i’fcS 015 

tbe op®reAtiajnal**patli* 

ijOih-- aJi)2Tiofiiy< pij£)c:£0iiaE3. 


lliia onetioii d«scril>®s tm procMupta* lirst proc®- 
dun* checks a liii® cut® a oircl® and io U 3 #d to cit«:k 

vhothar a link i» foyllniT with any ©ijurfeacl® C^* 3*3*2#li}« Ifc® 
®®€oiid procadyr® checks wl»tls««* m point 14#« Iriald# a trlawfl® 
and i® «s®d to con®id#r ca»#® of 3®c* 2*4, g« mm th« algerltiws 
haw h«#fi wlttwi as fynctl&na wi’Wi 


ii«f»fring to Wig* 5,1, mm fir»t imatUn is 

fyiiction liifi«jetits^clrel» 

^ ^ hoolaiifit (3«20) 

tN^in 

fmm cwtr® C»^*yu) ii»<* p«m«iidioiilar distaw® d 
to strfei-%lit li»« C3^f%) mA CJtg»f2^* 
if or KC* < tii«« iiiat^oiitsjsiwlsi-t^ 

slos 

if A* > rJ^ ti»ii Iiii®jButijeiwSl«t«i fala® 

‘WPP WPfr 

alflii 

if «igi® iX' is aeiit® '^@11 

linojButajeinslii im tmm 

tUm 


«ad| 


limjBiitsjBirel# i»» twi®| 


lit® srcofid pm»o«diir® is dswrihod no® (fig* 
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FIG 3.1 LINE_CUTS_CIRCLE 



FIG 3.2 POINTJNSIDE-TRIANGLE 



m 


fwnotioo (xVf^yVt axt'^y Cl* *5] of 

r»al| roal)£ ^iolomi {f*a| 

WMr 

aporfct mmy Cl*.** 3 j of l»clo«i| 
tiogiii: 

for mixcb mrtmx i i« 1 to 3 Jo 

b«H?li 3 

^oln fCl) to P 

if th« otlK»r tm wrticos lit oo oppoaitt 
»iiit of lint vCi)p mm 

aswrt £ij s«* trot 

olot 

a|3«rt ii] *• 

milt 

If fOr my two vtrtle®® 4 ajoi U 

opart [4J » tr«# CkJ •» tmm tbim 

point jUjaidt^triani'flt i* trot 

'i^St 

point^ifiaitlajtriaiii'lt • ftawif 

mil 

Ttai n^m algoritlis* liavt fot« writtai ija a Pa«eiOl»lili« 
md eoniNMioio to my atimtorti lanigti^,.:# i» oaay* iti® 
iictoal progra® e©d«» bav#, btm Mrlttao to BASIC, ©is m 

%l>lt 2 m niero^onpiitor and ar« fitm to 1X1* 



CHj^ER IV 


mmvsB Mm Qirnmsmmm 

4,1 immmxTiout 

fhp## nr® pjMisssited to ti^40 ctepttr*, 

In 1*11 thTi^ ca«»», th® lie* s to nectsa tfe« ittt«rt©r 

mils of diffsfmt o'lij«c%s# th® first tm prolal«s sr® meih 
thot th® fistlr® opsrsjtlonal paiai is oowml to ow #w«N@p, 
slisrsss, Im tos last prolAw tii« ot^'Set is i*»ti »ii»rlf 

hudfiiay torotieJi tli® 0|Niri4tiewftal path, ttia jw*rilpisl**tor ha® to 
tmtmet md a iioii><*op®ifattoii^ path hat to h# platsiM* 

llii algari'^ has twset toplsmastsi on m IIs 
nicro-HcciMpijts fha mume liattoi!; is giiraa to III* 

Th® progrm is of an totorsetiv® natnr# and higli t^solutioii 
sersiRi pagas ar» mad to tha ohdaot and tho amiiptilator 

on tha scram* Iho doacripttoii of tola totaractlv® pmgvmm% 
is proaontad to %pan£lix II* 

lha praamt lOgoritte dm» not ttcmunt tor solid adgaa* 
tharafora th# oh^acts haa® to ha iniittoly mMlad hy plaotog 
"pmuto>«obat®©la#'' to to® aorhapaea* SalaotSon of tha ohaimlas 
ii«tds a littla car® and soata gtiiilaltoai art giimi halair* 

1) If to# ohlaot is syMtatriCt it is hattar to |Aao# on® 
ohatoala at to® Itoa of ap^atiy ratowr than tm ftonhad aid® 
toy siAif a© th«»t tlia natwork is synaatoto# 



2) c>.iw»ii th# obatM!3.«s in siicti a nmy ass tliat t&« {ai#*poiiits 

of tl3« lie iniiidi# tis® wsHsioe s&mt of tho robot* 

3) %ac® thw‘ ©b»t4ssc2#a in «ucli a way timt th^ 
position* li« inaid® tb® oul«r poly^m fosucKi fcy tfao obstfiMSles 
or at l«ast r»t v*ry far awy eotsid# th® polft.wi* Uii;;. mswn^ 
sufficiont B#Ki«r points for tJi# algoritiifa, 

4) ''£m obstwcioa toitchiui,’ mch othtr ishoutld b«? 2t*op3.®e«4 

by on® obst&cl* itiwlc^lng botli* 

3) It posaibl®* tty to k««p th® t»m 0 ontsidt tis® oiit#r 
poiyison foitMid by tb® obstaoios* 

km2 Qmmt mu iHiibE HimHion mm^m 

Iti« objtot is rtiowo in fig* 4*1 i$ tbt wno as iis«fi 
in i^f* £4j« Tim #nil*»®ff«otiir bss tst tfsok n pite wbieh is 
sp®oifi«d by ttss sirtiigiit linss fining tli* points 

^Inis* tlm t^smtlonsi psl^ is sbown in !%• 4*1 by chain 
XijmBm fbis pstb is trwicsd by tokljig tld.$ty tmsr nsariy 
squHilly •nd’-sffictor poidtims on Vm paHi* 

Tbs objset is nodsIlsiS by piseisg fottr obtte^eiss in tbs 
morbspoes as sbown in fig* 4«y« Obstsc^ss 2 sii4 3 bsyt bssti 
plniisd mt tbs opting of tbs enrslty to mmi4 a collision# 
Obstiteiss 2 and 4 bavs bssn plsasd at sitbsr aids of adg# 15 ^ 
inoliids tbs spaas inaids tbs a&wlty as wall as tli* spats otitaids 
it* into tbs worbspata* ttm trlai^lss 225 tnd 2^ is tbs sat 
aslstiad to imm tbs 'natwork* not of trtai^ti 2IA and 2^ 
is t«^a«tsd as tbs txdjntlls 254 is ^ fiat md ibis will iMto«* 
ibios difflaiiltiss in followiag tbs anw of ode# fig* 4*2 
aiso sbowa tbo avtows* with diraetiofni aisiilMd to Mb adi# 
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occoftlim:? to tb® nam ill mCi, 2m3*l* 

ThlM protlwa wmm a©3L’i»^ c©iisi4«pli^; tta»34'3«2.oto3f*s vitl^ 
3f****i^'f6 B liii^feis* Iii Autii til® Qvm &f of all 

tt*® links i« kfspt sas® moA U to W «its* fiit tl-Ji®® 

Ilisli aatl til® foiMf link tswaipwOLotora vmn fomd to mctittmtvilly 
p«‘fora tti® to8k« Wi® r®»iat» or® piiia«fit®d onX^ for fl-w, siac, 
and «iijht link mmdpxjimtors* 

7m ca»®s of fiv® link MMsipiilators are conii^owifc In 
the first eaa «9 all the linic lenirths «r® •qtwl aadi aw ¥j units. 
Ill# conflgimitioiMi for positlofis 5* 6 ani. 14 me^ abo-m in 
Fig, 4,5 positions 1% S9 «a«i 34 aw 8l»i« in I’if, 4*4, 

Fig* 4,i shows sup®ri»^»®a alteroat® eonflgawtloiis for th® 
entiw task. 

Fig, 4,6 shows til® Besisr Faints E| Bf C and B fur 
the posltioii nwlNr 29 of tlM flii«l»sff®€tor« iks is orient fm 
this figtiWt yhim % Unt ®<l^® 23§ Bexisr 

point ahniptly ohang®® frm a to B* this wmm» Xm^m rotatioiis 
of til® joints, partioularlir of the joints iMatiNl towardis the 
Mi^ef fsotor* A s®tho4 of saoothwiiiig this twnsitJte at 1±i« 
end^ffwtor siB® of th® »siil|n<aator has heen niM* For this, 
conslilar list mpmmmtmS, in Fig« 4«S«' ^ noetwst #%« 
to E is 95 whoso ni(Hlioiiit has hwn wpwsontiil m point jW 
iiOiito Mdglo hatwssn th® litis of orvow at piifit 4 mmI lino £4 is 
oolomatait* fhia anglo is 4hoii« as # in tha flgiiw* Thm^ th® 
ntoit B®iii«r poiht is ehoa«n at 4 ' lsist«ad of 4 4 ' lies on 


lino 4B amt 


» jBS O0S 0 


( 4*11 
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FIG 4.3 : 5-LlNK MANIPULATOR 

POSITIONS 3,6 & 14 



Fig 4-. 4 ; 5- LINK MANPULATOR 

POSITIONS 1^:.29&34 


7k 



FIG 4'5 : . •5-LlNK MANIPULATOR 

SUPERIMPOSED COrNlFlSURATiONS 
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Idea] Points (Q^) 

Joint Positions i 

.Bezier Points 

? OA ,73 

206,71 

204,71 


2 73 , 99 

173,97 

162. 

100.06 

125,1 a 

137,114 

, 167,03, 

105.00 


101,130 

131.25, 

106.54 

65,137 

61,136 

.17.62, 

19 C . C '3 

90,108 

90,108 

90, 106'; . L„ 


FIG 4 6 : BEZIER CURVE AND MANIPULATOR 
CONFlGURATiON FOR CASE IN 
PROBLEM 1 . 




polfita tii# Joint tiegitions ttad Hie Deaiftr point&i , . 

#ill thif r<&gulta| emiwpt 0m (with fiw wawiqiial pwaetitffd 

ill tills mrk imm b«m na «3!plgli4®i iilwf* 

l.«xtt a flw® link »tiiiipiilat©r with i»»s;«cd links la 
atwtiiad# Th« link l«ingtls» ar*© « 60* "* ** 

^ 1 # “ % •«• 2u» ffe® awlai*! Joint rot»tioii« toaw l»ti» lomd 

to b© 35 < < 172* •*158 < #2 < -92* -60 < 4 57, -9 

-22 <. #^ < 39# 7%* 4*7 lim ooofigwatlm /' 

at iDOaitlona 18* 29 and 34 witltout «»©’tti®nlKiC' iiirst 
B#Ei«r point in tb® liat# fig, 4,8 nlKiwa tli# «^b» a«iipiil«t©r 
at tli» amt poaitlona# but incox^fotitic tb® aaootbining aobaift* 

Plata of Joint maglm Ibr tb» f iipo lliA# «anipniato» 
with •ni-affao%»r poaition ninibar ans abown In llg* 4*9, l^'ig, 4«9a 
ateim tlia waftation of tba fifth Joint aii^l® with poaition nyttbor* 
Tki« fi^uuna ahowa plota with tnunpal linha iwoipoiatini Moothanr 
ing* tmoqiial linha without saoothaniitg iiii aqpil with 

maottiofiing* It ta mMmt that Joint mtsitimm wiiteut anootten^ 
lag. haw® mry lafga wariationa* fii* 4«9b wtem th« plota of tha 
foiarth Joint anglaa with a^affactor poaitisn mbar* thima 
oaaaa ea nantlonai ^irliar haw baan «oi%iUtotwtt tnd it ia 
obaarvad thtit joint wsfiXa for tha mmpA IJM with •im»^)aii»* 
ing baa langar tnriationa in thia aaaa* Shit i» m bataua® tha 
:aiiall«r wrl<!i^4iMt of tha fifth joint taigla is®» to INI *©i^pi«i*tta4 
•Wia fourth Joint «n,^la# Fig* 4,9o ahoiia ^ ^plota of tiia 



FIG 4-7 ; 5 UNEQUAL LINKS 

WITHOUT SMOOTMENING 



FIG 4-0 ; 5 UNEQUALf LINKS 

WITH SMOOTHENING 



Fifth Joint An,<;lG (decrees) 
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FIG 4.9a : FIFTH JOINT ANGLE vs 

BE POSITION NUMBER 


■fourth Jolnl; anf 



5 unequal links, v/ithout smoothen 
5 unequal links, with smootheninf 
5 equal links, with smootheninn; 


Lnd-effector nosition numoe 


pig 4-9 b; FOURTH JOINT ANGLE vs 

pp POSITION NUMBER 
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fig 4.9c: THIRD JOINT ANGLE vs 

££ POSITION NUMBER 


tM's'rt ambles with «sr«!r®'ff#ct©r pasittoo fsiialJer# C&mp&rlBm 

of ttm two c*-s^d of flw iMoqual IteK wiidpia-^tor witl»ut 

anti witlj ?3!3ooth®niafr) rwoals th^t th# tfftet of Kteothoniii; is 
bwjfltly rofl*ct#st at th* thAM ^oint* It ma iQm4 that the 
firat and the second Joints also retain nearly a£s:.» cK>tlona, 

The ahoiRt pioblea ties aim tmm solwd witls a le*linli' 
md a f>llnk raimlpuilotor sad th# Joint snirles liav# he* ooia- 
pmxwi* The plots of th# li«t Joint aaagles with en4*#i'fect©r 
positiofi for Ids# 5t 6 end 8 linh iMipulatora pm&mtmd 
in fig* 4i«10s« It is ohserwiS ^^it ^e lest Jaint ni^l# for 
all iianipulstorf!i Ims the mma mtmn sad dost not vary siiini*** 
fiBURtly«) A s^i^insl deoreswe in Joint rotstAcsii is evi<tent if 
the ntmher of links sre increased* fig* 4*10h the plot 

Of the Cn-?)'^ Joint iingle fro* the eiid**effeotoi* iidi with ^Eid* 
effector position mnihert where n is the ms^r of linlts of 
tl^ mmipiiXator* It is seen that the fttriation of this Joint 
lu^ie deeresMs as the aunher of links are ineriMiod# Four 
oonfigypitioiss of the si* link nanipulator during the task, ■ 
ss«e shown in Fit* 4«ll* 

one lost earistion of this prolil#» hue %m» tided with m 
>»liiilf nsniptastor with equal link hut hricsing the 

hose nearer to the workpieoe* TIm oonfigumtiMto for positicws 
6t 15 «nd 27 sre giren In Fig* 4«lt« m otgiooteOi the joints 
iMied mry Iw^e rotations end the esniptilator eonfigiiititioiis 
look iiipniiitiiiia« ho sttenpt is iisd# In this wmk to plmm the 
iNSst of tho wMipulotor st sa ideOI loiSlticft* a Ofitotion to 



I,ast Joint anf;.le (define 



FIG 4.10a : LAST JOINT ANGLE v 

EE POSITION NUMBER 






FIG 4.10 b ; (n-2)**' JOINT ANGLE vs 

EE POSITION NUMBER 






FIG 4-11 : 6-LINK MANIPULATOR 

EQUAL LINK LENGTHS 



FIG 4-12 : 5- LINK MANIPULATOR , EQUAL , 

BASE NEARER .POSITIONS 6 13cS:27 
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umtmrmitm mi optiaai md of thet 

totwl a««a«) i» al»o isot 

Th# i;oi^uti*tio£t tiw for tlai y# liitil'. 

Bwiipifl&tor aoitttioas os «i #^pX# XX« a4cro-ec®pM;feg'r mrt fciwfi 
teftlow'* 

2 •* Xitik iUBfputlator 55*5 s«c* 

6 - llsJ' latisipi^a'tor 6 ^i»? wc* 

8 •* lisit »«f3ip'ulat©r 73*3 s^c* 

It is ««#s tivit coisputatios tte# itoea mt Imrtes# 
prcif»rtl©nat®iy witl^ th® nwlser of links* iKiroast is 
coi^^utatlon tin# i# hmmm mtt&r '«Mh eosfiipJtrotioii i» 

BiiMNi, th« chocks «r# nad# to •asuar# acillisioif»fr©# co»fi|;«» 
f«tlon* &‘‘Ch link is chmokmd with l•csp#ct to noch ohstsclt* 

A 'Hodifieatibfi css %»# iseofpoiwitiMl to iliecimM tliis cdcliticsfial 
ci^^tstios tism with iscrcas* is sMihtr of Itite* 

4*3 OBJECT Vim r»E IBTEaiOSi filftI38 

This object is show is Fl«* 4«13* Tli# wiffespice soiel 
is show iax Fig* 4*14 m»A is siailsr to ttet of Fig* 4*t« Mew 
tho efpmmtionaX paths possini tinoogh points sM imrmm 
th# cosg>liQEitF of problse* Hi# pmbliiii is soltwA by osisi^; 
m 3*li^ iM»ipiiIator* Fig* 4*15 ahoiM th# conficpifutioiis at 
pooitioii 8 BKKl 19 i^Kil# Fig* 4*18 show tho cofii%jtifotloiio 
ot positicitis 19# 34* M* %o Msiilpil«tor i« 

8otMn!!^h#it ot fortyon# o^ioslly apso-od oiMl~«flfootor positioag* 




FIG 4-. 13 : PROBLEM 2 •. DEFINITION 



FIG 4-14 : NIODELLINS OF WORKSPACE 






FIG 4.15: 5-LlNK MANIPULATOR, 

POSITIONS 2,9 & 13 



FIG 4-16 5-LINK MANIPULATOR, 

POSITIONS 19,24 &31 


position iiuiiil»®r 31 1 th® jxjoltiou Is 

m 8i50«i iis Fig.' 4*l?a* At this position* is not visible 
fiw 0 mid so fflorksr i»oiia to Iw pot at tl» previews odti'O# 

Xii tills o*4s« tile ®tti''ls@r has h^i put at tho odi'.o li® s2io«a 
■ in Fiif;* 4,lTh. itoothm' strategy for puttlui' ai.ir!iera Isas been 
adopted here wSnara iiecaasttiy* At position Cl'lu 4.1.7a) „ Ito# 
fxjint i® vlaibla Xross 4 and » a marker ha® uot isiwis u^td 
anyid^re* But lir*». £Xvm of tfc# conficii3»tloii oollide® 

with otosttacl© 1 as alwma th« fis!!i line in 4#l?a. In 
*iiLh a case a raartcar ha® ba«a put at that edge mntMSsm 

obatacl# 1 at its eM, and wiio®# nid-'point i® fi«a 2 '’«»t to 
obat»;.cl« 1. I^e edge in thia oasa la 13# anl m a aajrtser l»s 
h«»fi put at B a® alKswi in llg* 4*17c. Hi® r®8iiCI,tto|; ©onfigur 
imtioti# for mt^mtioetor at ia i#itli a daslasd lim in 

Fif , 4»lTa and la trm of orif colliaioii* 

4«4 OMRCf l&auXRlNO IftilM^EIIATICilAl. Fatfl FLMIlUI'/t 

the third pmtfLm is m spray*paiiitii« pjwblaes i^ioh laity 
oocur during a mr paintiitg oparatioii# fig« 4«ia ^wi th® 
aid® pamil of a mr whieh ha® an ohataelt m%mm t»io 
idJKbva. It ia r®quir®d to paint th®’ «ntir® roof p«m 1 of 1d%® 
®ar paaolm ttireuili Hi® window# Fm^otad in th# protAm 
would rnppmr m atow in fig* 4*19* 1%® wincl9ws din® opm spmm 
tliroi«h M,oh tim robot am ia fro® to go insidt «®r* Hi® 
taalL ia d«fiii«d by spacifying th®' ondnifftetor poinM ^ to 
tiMNs® potata tiiiai daf in® th® " oparational patt \ ft# ladi^ini 
of Hi# worle^a®® by pieoing obataclaa is alniwii M i’lit 4«a^* 
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FIG 4.17a DEMONSTRATING MARKERS 
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FIG 4-171:, : LIST FOR FIG4.l7c :LIST FOR 



obstacle 
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F16 4-. 1 8: PHYSICAL DESCRIPTION OF TASK-.PR0B.3 



FIG 4.19; PROBLENA DEFINITION 
IN 2-D 


rn 



FIS 4-20 MODELLING OF 
WORKSPACE 




I'iiia pM»bl«a hmm hmn soaiwd with a ®ai3lpu3.'-itor «<! tts# 

moults art prtatnttd iis 4*21 tl^rowti.-h Fir* 1%, 4*?1 

aiiows ijoaitlaas 6# 12 lawi 16 af tlit niaii-pulatoi:'** ^* 1 ; position 1^, 
tlia ftitoipwlator c%«iiiiot ac^etss tfe® litxt «nl?-©fXcs:;tor* 
yltkmut colliiilHi?: with ohstatlt 2 if it go#a iii vXxi^w 1 

Fl|»* 4.19 ««! 4,30). til# f»n*optr«iti 0 iia*i patS: is ww 
#alculat«i*i oirciittvent th# o&»t*.ACl# 2* atid ite i*owc?-^^ iiatiirt 
13 sliowii in Fig* 4.K.’ hy <Jtoubl€ chain -iatttd'lix;©* *iSi# cctiiiCl 
operation#! and now-opowtionsi paths* as CiilciAatod#, ar# aliowi 
in frmti} of Fli;, 4.22* fh® total ntraher ttf «il olSmtor 
position® on th® ©periitional «a4 no 0 *op*witiona.l patli io 57 wad 
tlisse points are equally spaced* Fii:, 4*23 poai^tioR 20# 

25 and 27 which lie on th® pate* Fig# 4*24 »l3©i» 

tlir#® positions on th® iwnropsiwtioiiaa, path wln» it is iipprewshii^ 
tb® p»jml tbrouili wii^w 2« Fl^t 4*25 ®l»ws po«itl««® 13# 9 
acid 45 on tli# opemtional pi44i wtei tte Mmipiilalor is MtiMsting 
to its startini position. Cmifigiiratlona at mfi»m stsfoa 
of th® task liuv® l»««i €&ll«cti®®ly shown in 4«24« 

A Mdor differwnc® in tM tftro® proMoss prtawtod ia that 
in th® first two ea®#®# I4i® «ntirt «wHiff®otor tps^oetoiy lie® 
inaidNi^ tb® oiit®r polyt®c« Hcwowor# tb® ohgltttlts for l^b® oa3r**ioof 
painting prohlaei mm plae®d in such a my tlmt thim or® .poaitioii® 
of th® •n4*»®ff«®tor whioli li® owtaid® polyi mi hot ar® 

ntlll in non®® t to 4* In such ea®®«» if th® of 

€a«® 4 C'ioo. 2«4#f} i® foUowoil* wmlpiLilator w0y <»lli<l« 
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FiG 4-.21 ; 5-LlNK MANIPULATOR 

POSITIONS 6A2 3-1S 



PATH 



FIG 4.23: 5- LINK 1V\AN1PULAT0R 

POSlTI ONS 20,2-5 & 27 



F1G4-24: 5-LlNK MANIPULATOR 

POSITIONS 2a, 3 3, 34 
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F!6 A-25 : 5-UNK hAANlPULATOR 

POSITIOMS 35,39 6^4-5 



FIG 4-26 5-UMK fsAANlPULATOR 

SUPERIMPOSED CONFIGURATIONS 


with *a obstael®# fo aimi4 thi© p©iaibi2ii;>- i^tsej/etfeer^ a, 
Iseaier point ia plaeait oa th® arTOw oorwapoati.lJ.;, tc tla^ 
c«ii:e to iteao 0 ia 1^# Xist, list ^iattiaxso ol ti^la 

iioziar point from th» centre of ojtow Is tal»i ao tm «sci 
hoix' tia«3 ta@ length of toe arrow* 



fitliiFfEll V 

coi«:lusi©iis 

A m^thoA is propowsd in tiiis woffe to tlj^ 

pTQbim oi 4»imminiMif: ^oint «tigi#s ot m mmiJLpi^ut&r witti 
reduKdAnt link* for m giwfi rn^mtfm^r positiox?.# hm6mid&xkt 
links irt u»®4 to cireaaiwnt ofeatocl^s in tis# %k»f!:scU'3c:t* fhr«f, 
diJffsrsnt sawplta in Incxmateg o*tK»r of difiieulty haw fown 
s4icc««®fuQLly i»0iv<Ki ijsiiii; th# propo»s4 aigoriti'fiit tor «wah 
mmmpHOf tiis m^&^ifotst&r i® i«siiiijp»4 to trao® sar^m ifi»M® 
mmo ©toj#eto# 1l» ptith ia appr®3ciWLte4 hy imffi** 

oisntly mlom pNoints ssed to« saf« Msil^u^uter oonfiinvitttoa is 
St •ach pnintfr I'urtosr^ m Im® hton illikr* 

trtttod to a when* tte sn^^ffwter sc««ss 

th# n« 3 ct point ftwi tkm pr#»«»t coisfiguawtioii witliout th® link# 
with floM obotools# Bmh p»s«® tew tito hwn *M£Cfn 
tar# of by planniiif a iwit^pwattoitf^ path for too otehoffoetor* 
Ibte sOnorltlia dtetowtiios swoeosaiw ®aistpul«tcir toidtiois® tor 
tlii# i3».w«.opir«tl0ii®l piitte torttoi lAlob tte iwiR^piilator rotrocto 
«ii4 approttcboa tot »«i® footer poaJtlon <aft«r circmrtntliii?: 

obi^elo®* In outoor^s this of oaa# tea not 

testi teiit with in aorlitr slteSios* 


iMlik# provtoMsly tN^rtte IftorotufOt to# pmpom^ 
oli?oritos is not Knob m too miiixtr of llate Ia tte 



tt siaitfekiae Msooth cuw-i- m^idixn: t-ill tli® ot>dtttCl«»* 
Tbis enir« ifs iR4i|i«Es#i»t cif iii»b€*r oi‘ IIbas# ^-“ipro^ioating 
tlit^ curv© by th« iitika ia cam Ii*i3 lice-: tai'.c^i 'to 

recb^k far colJ.isiAon* 

It waa obstTv^^i tisat by the nKTiber Ilnka^ 

while keepiiig the to'tal ctmafemt, stations 

ef the 3ial«3 ©h^^ei’ftNBtor €« tee llw Iloks 

tovarih baa© oosti/ m^i4*Xect#i* Beea«;to rsw-ter 

mw3ewra,bi:iit!f la required Oh the »4*-effm3l».r sit*#, the 
terrw.'iri© In nwaher »£ llhKs distributes the towards end 

of the sa©aip'ilat©r« 

Firially* tot prhptttd nttotd it to alMys yitM 

t safe cthiiguitition of th« if ayob a configuration 

is ft&sibl©« 

teiw. ot 

Instead of isodtlliag tli» objtct® with diwrot® circdlsr 
obstoelos* cooctpt of bsrrltr in%«s iteiild be iMcli 

itisiirts that mrmm at tiiat edge alwiya point towiitl the turn 
w^mm tht factor* 

QMmm tlio total itcfti} of the iMs* totomihation of 
tilt optliitaii mislitr of links mid optintai link wuld h#to 

In Mtntifyini tits idtal nanipulator iultod lor « toA» 

Instly, for tfeoli ©b^tot or toi^* tomro «»f fet an is^tiawi 
tositicnt of tilt bate of omilpiiiator so tost tho toA in pmnfoxtttS 
wito isiiiimm doint rotatlona* 





3.* a«ijS ;3ofii, ^f;oritl»a fw me^utmr 

i^nXdmm6 of a aiaiKipiiifator ia br‘twa®ii obstoela©, tloiroaX 
of l4S|^te««r|jai:, for Xadmtrft -n^rios ir# 97i3)» pp*636*^fe^2» 
1973 *' 

2* Khatlb* 0*, ubatacle AiwidUmo© Xbr r.aisipiAat»r® 

4ji#i4 i-loteil® iiiobote, lEiX Isiteroatloaal Confortoee on 
Robotics and ^tooation* pp« ^ii>*3u3, 19B3* 

3* H* mad Vuflcobnitovic* 1^*# *r«5«!5toi:y Plijyp»iisi: 

for ffet#«Kl«fst lli*olpul«itora In tkm Proseiwo of Ubataesloo* . . 
ihoory cmd Finactict' of i-iofoots and Kanlpuletors* Fro«# 
of 8oi'liMn%*S4i SbMe Fifth :3p!ipoaibCi» 

S^* 3?-*65i. 

4* K asici i'□.#1^3.^ Charlos A*,# Obstacle 

A-voiMOMso for ltlii«iiati©ally jit«tyn«lmt llneipwlatorii in 
Bpiiiiiio^lf tmryljig EnviroHmontaj 3lie Intoxmtional 
Jourottl of fttaearch, Vol* 4, flo# 

Fall 1983* 

3m BrooliSf ^dmy *» md kiMw^Poroti ToM#f 4 AMivliioii 
Algori'^m in tonfig oration %m(# far fli9^%Ntlh iHith 
i^tiitioiit ZBiM m Ite wd Cibor* 

notics. tol* a«-15, Ho. a# pp. 2S^23% 196>* 

6* BrookOi 4*^ ilolvinii tis* FliMl^iit& Pm^Xm Gmd 

fteroaontiition of frm %ao«* IBFE tr«ind«»tl4m@ on 
Man and C^iarmtica# *F©1* a^l3§ 5# pp# 198«»'196f 

?4arcli/%r!l, 1^3* 

7# Ctiralty GiMii<i«a Mobil* Bobota* Hoiotlo# md ArUfiOiol 
Iti'^IligMC** Mfo 431 3*ri*0t %riiii*r»7@ilM Basliii 
H»ld#lfe#rR, mm torkf 7ok3«>* pp* 

8* Fsaeteor* Jaroalmirf iteidbook of litmarioid. 

Appiioatiima with Frogsratt* for lufiiiaors md 
Hd&mm liill Go* a l^it 
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mzmi cimvis 

A £^8l«r eww orifr a is w-pf^wnt#! bj tbe 

?(u) . ! 7, «t, (l-u)'”^ j- (M.l) 

u ia a |mi«a#t»r t3mt varies along th® tiio 

oorw* Co < u < 1), r. ia tli® v®®tor eoordiiiistoa oi tb# 
i»piit point, md rCtt) r^w?3«nt» tM vector oooi'tHnatti of a 
point on tbo oum^* 

A einrw ©f oi^r » .r©<^iir®s Ca^l) point® »M«fe 

fiii^ !»• eaxiM Points* ^ Btsior <&mm m&d mt 

iM'Osaarilf pass tbiot^ii tti© B«£i«r points* fig*^ Ai^l ®boi«s a 
plmmr B«*ior ciirv» of opdo,r 5* Hio pamoton 11 io mmm at 
«itd' oQuia to 1 at At otb«r intmiitaiato poiiita 
0 < » < 1 * 



I|«« A3L«it Booior €wio mt Mm % 





If (( 
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APPENBIX Ill 


1 

RE-M 


2 

REM 



3 

REM 


OBSTACLE AVOIDANCE * 

4 

REM 


PROGRAM » 

5 

REM 


« 

6 

REM 


Anupam Bagchi « 

7 

REM 


Dec 1986 ^ 

8 

REM 



9 

REM 



10 DIM XE(15),YE(15>,XB(10),YB(10),RS%<10),XI%(20)rYI%(20>,AA(10> 

15 PI = 3.14159265:BB = 180 / PI 
20 REM Cubic Bezier Curve 
30 TEXT : NORMAL s HOME 

40 VTAB 4iA$ = "CHOOSE INPUT METHOD" s GOSUB 9000s OTAB Bs HTAB Ss 'PRINT 

"Press": VTAB 10s HTAB -8: PRINT "G to give input graphically": PRINT 
5 HTAB 8: PRINT "D to read from disk": PRINT : HTAB 8: PRINT "K 
to type from keyboard "? 

45 GET KE$s IF KE$ = "G" OR KE$ = "D" OR l<E$ = "K" THEN 50 
47 GOTO 45 

50 IF KEf = "G" THEN GOSUB 3000: GOTO 150 

55 IF KE$ = "K" THEN GOSUB 1200: GOTO 150 

60 GOSUB 9500: GOSUB 1100: GOTO 160 
150 GOSUB 1100: GOSUB 8000: GOSUB 8200 

160 GOSUB 41000; GOSUB 10000s GOSUB 10030: GOSUB 10550 

165 REM GOSUB 50200: FOR SS = 2 TO P3%: HPLOT XBCS3%(SS - 1 ) ) , YB < S3X (SS 
- 1)) TO XB(S3X(SS)),YB(S3%(SS)): NEXT SS: HPLOT XB ( S3% < P3% ) ) . YB < S3X 
<P3X)> TO XB<S3X<1)>.YB(S3%<1)) 

170 PRINT : PRINT : HTAB 7: PRINT "List forming convex polygon": PRINT s 

HTAB 7: PRINT "Obstacles FOR JJ = 1 TO P3%: HTAB 17: PRINT S3%< 

JJ): NEXT JJ: HTAB 17: PRINT "..in sequence." 

180 GOSUB 10700: FOR HH = 1 TO TR%sUT$ = TR1i<HH): GOSUB 11000: NEXT HH 
185 A$ = "Network is ready !": PRINT CHRf (7): GOSUB 9000: PRINT : PRINT 
: HTAB 7: PRINT "Hit a key to plot network GET KE$ 

190 GOSUB 50200 

200 FOR KK = 1 TO TR%:V1X = VAL ( LEFTIi ( TR$ C K!< ) , 1 ) ) : V2% = UAL < MID3> 

<TR$<KK) .2r1) ) :y3% = UAL < R IGHT$ <TR1i <KK ) , 1 ) ) : HPLOT XB <V1%) ,YB <01 

X) TO XB<02X) ,YB<02X) s HPLOT XB<V2X) r YB C02X) TO XB < V3% YB < V3X ) 

210 HPLOT XB<V3X) ,YB<03%) TO XB (U1%) , YB <V1%) : NEXT KK : GET l<E$ 

220 FOR KK = 1 TO EDX; HPLOT TX(KK) ,TY<KK ) : GET KE$; HPLOT TX ( KK) r TY ( KK ) 

TO HX<KK) ,HY<KK) s NEXT KK 

230 GET KE$: TEXT :Af = "Are you satisfied with the results ?": GOSUB 90 
00 

240 HTAB 15: PRINT "(Yes/No) 

250 GET KE$; IF KE$ = "Y" OR KE$ = "N" THEN 265 

260 GOTO 250 

265 IF l<E« = "Y" THEN 900 

270 PRINT : PRINT ;Af = "You may start again by changing": GOSUB 9000 :A$ 

= "the initial scanning angle.": GOSUB 9000: PRINT s PRINT "YOUR DA 
TA IS STORED IN FILE : "?FI$ 

280 PRINT : PRINT, : HTAB 5: PRINT "Hit a key to restart execution GET 

''''.KE^'Sr RUN ' 

900 GOSUB 2000 , 

1000 END- 

1100 TEXT : HOME : OTAB 5: HTAB 5s PRINT "Starting point : ( " ? XO ? " » " S TO : 

")": PRINT : HTAB 7: PRINT "Number of end points = "?EC%s PRINT : FOR 
JJ = 1 TO ECX: HTAB 7: PRINT "End point ";JJ;" at ( " j XE< JJ) ? " , " ; YE( J 
J);")": NEXT JJ 

1105 PRINT : HTAB 8s PRINT "Press RETURN for more ";s GET KE$: PRINT 
1110 PRINT : HTAB 7s PRINT "Number of obstacles = "jOCX; PRINT : FOR JJ “ 

1 TO OCX: HTAB 2: PRINT "Obstacle "jJJ?" at XB ( JJ YB ( JJ )?" ) 

with radius ";RSX<JJ)s NEXT JJ 

1115 PRINT : HTAB 8: PRINT "Press RETURN for more GET KEI*: PRINT 






"Number of 
NT "X obst 
acle C";KK 


"Number of links in njanipulat 
HTAB 8: PRINT "Length of link 


YH:XM = XO:YM = YOsNSX = 

I-:.. ‘ 




IF KE% ^0 :dR KEX » <68 THEN 3350 
IF KE%'" '90' "'THEN ' 3500 

IF KE%'^^=« 70 THEN. 3a80:':’;:-.-;''';-;v.'Lf^H- 
IF KE% « 83 THEN ,3390''n'LT".';;.;;::j^f^^^ 


2900 

2910 

2920 

2930 

2940 

2950 

3000 

3010 

3020 

3030 

3040 

3045 

3050 

3060 

3065 

3070 

3075 

3085 

3090 

3095 

3110 

3115 

3120 

3130 


REH error handl ing rout ine 


IF XJ > 279 THEN XJ = 279: RESUHE 
IF XJ < 0 THEN XJ = 0: RESUHE 
IF YJ > 191 THEN YJ = 191: RESUME 
IF YJ < 0 THEN YJ = 0 
RESUME 

HOME :IC% = 10:GLX =. 1:0CX = 0:ECX = 0: 
0:NLX = 0:KIX = 0 
H6R2 : HC0L0R= 3 

BL$ = CHRi^ <7):XH = 140:YH = 95:XQ = 0; 
GOSUB 3150 
GOSUB 3980 

IF KEX = 27 THEN RETURN 
IF KEX = 8 THEN GOSUB 3190: GOTO 3040 
IF KEX = 21 THEN GOSUB 3230: GOTO 3040 
IF KEX = 88 THEN 3600 

IF KEX » 10 THEN if 3270: GOTO 3040 

''IF" kE'jC :'??;;*77,‘THEN.i'355Q 
79 THEN 3410 
IF KEX * 11 THEN , GOSUB 3310: GOTO 3040 
IF KEX V 65 AND HEX » 0 THEN XO = XH:YO 
1: G0T0'f304O''tl;: ! _ 


=. 1 :OCX = 0:ECX = 0:X0 


= 95:XQ 


0:Y0 


0:XH = X0:YM 


0;Y<3 = 0: HPLOT XH.YH 


GOTO 3040 


11 

5 

0 

p 

R 

I NT 

: HTr 

iS 

6 : 

PRINT 

12 

G 

0 

R 

H- 

H ac 

:cepti 

5 

i npu 

t fro 

12 

1 

0 

H 

0 

HE : 

: UTA3 

5: H 

TAB 6 




T 

N 

PUT 

■■'X = 


:X0; 

HTAB 

12 

D 

0 

D 

3 

INT 

: HTAB 

3: 

INPUT 




K:< 


= 1 

TO EC 

-f m/ 

: PR 

INT : 




XE 

( 

KK) : 

! HTAB 

8: P 

RINT 




m 

P 

RINT 




12 

3 

0 

P 

R 

INT 

; HTAB 

3: 

INPUT 




rv 

w ^ m 

z 

PRINT : 

H 

TAB 

8; PR: 




m 

HTAB 

8: PF 

il 

NT " 

Y obs' 




p 

R 

INT 

"Rad i 

u 

s C " 

;KKp": 

12 

40 

p 

R 

INT 

; HTAB 

3: 

INPUT 




KK 


= 1 

TO NLX 

2 PR 

INT : 


^ress RETURN to continus 


: GET i<E$: RETURN 


; coordinates PRINT : HTAB 10: 

r = ":Y0 

:nd effector positicns = ";ECX: FOJ 
iT "X end i:":KK?"3 = IN = UT 

:"3 = INPUT ""jYECXK): NEXT KK 

k 

!b Stacies = "jOCX: FOR KK = 1 TO 0 
:1s C"?KK?"'3 = INPUT '"'jXBCKK) 

'3 = ";s INPUT ""jYB<KK): HTAB 12: 

■ "";RSXCKK>: NEXT XK : PRINT 


"";AA<Ki<): NEXT KK; PRINT 
1280 RETURN 

2000 REM store the network on disk 

2010 D0$ = CHR$.(4):F0$ = Fl!& + ".NET"; PRINT D0$: PRINT D0$; "OPEN" ;F0$: 

PRINT DOf :"WRITE";FO$ 

2020 PRINT XO;",":YO;'%"?ECX: FOR JJ = 1 TO ECX: PRINT XE< JJ) j " ?YE< JJ) 

: NEXT JJ: PRINT OCX: FOR JJ = 1 TO OCX: PRINT X8 ( JJ) ; " , " ; Y8 ( JJ) ? " 
jRSXC JJ) ;","!!RD< JJ) ;","s;AN(JJ) : NEXT JJ 
2030 PRINT NLX; FOR JJ = 1 TO NLX: PRINT AA<JJ): NEXT JJ: PRINT EDX: FOR 

JJ = 1 TO EDX: PRINT ED$CJJ);",";TMXCJJ)?",";TN%<JJ)?",":HX<JJi;",";.- 
HY(JJ)?",";TX(JJ):",";TY(JJ);",";EX(JJ)?","?EY(JJ): NEXT JJ 
2040 PRINT DO'k ; "CLOSE" ?F0$ 

2050 RETURN 

2760 ONERR GOTO 2900 

2770 B8 = 57.2957796: FOR JJ = 0 TO 360 STEP 12:XP = XJ + RJ% * COS (JJ / 

BB):YP = YJ + RJX » SIN <jj / B8 ) ; HPLOT XJ,YJ TO XP,YP; NEXT JJ: RETUR 




3170 

31 80 
3190 
3200 
3210 
3220 
3230 
3240 
3250 
3260 
3270 
3280 
3290 
3300 
3310 
3320 
3330 
3340 
3350 


3360 

3370 

3380 

3390 

340Q 

3410 

3420 

3430 

3440 

3450 

3460 

3500 

3510 

3520 

3530 

3550 

3560 

3570 

3580 

3600 

3610 


3620 

3980 

8000 

8010 

8020 

8030 


FOR JJ 
180 STEP 
RETURN 
IF XH - 
IF HSX 
H€OLOR= 
XH = XH 
IF XH + 


= 10 TO 270 STEP 10; HPLOT .JJ,0 
10:' HPLOT Q,JJ TO 3,JJ; NEXT 


TO JJ;,3s NEXT ; FOR JJ 


10 TO 


'HEN PRINT 8 Li; : , RETURN 


RETl 


RET 


RN 


IF NSX 


rURN 

: RETURN 


= 68 AND KI% 
I^4T <YI%<KI% 


1 THEN 
1 ) / 


IC% < 

■1 THEN 3223 

0: HPLOT XH,YH: HCOLOR= 3 
ICX: XH,YH;N3% = 0: 

IC% > . = 279 THEN PRINT SLi; 

1 THEN 3260 

HCOLOR= 0; HPLCT XH,YH; HCOLOR= 3 
XH = XH + IC%: HPLOT XH,YH:h4S% = 0: RE 
IF YH + IC% > = 191 THEN PRINT BLi; 

IF HS7. = 1 THEN 3300 
HCOLOR= Os HPLOT XH,YH; HCOLOR= 3 
YH = YH +10%: HPLOT XH,YH:NS% = Os RETURN 
IF YH - IC% < =4 THEN PRINT BL$;: RETURN 

IF NS% = 1 THEN 3540 
HCGLOR= 0; HPLOT XH,YH: HCOLOR= 3 
YH = YH - 10%: HPLOT XH,YH:NS% = 0: RETURN 
IF NS% = 1 THEN 3040 

KI% = KI% + 1:XI%CKI%) = XH:YI%(KI%> = YH: IF KE% 

HPLOT XI%(KI% - 1), INT C(YI%CKI% - 1) / 1000 - 
1000)) * 1000 + .05) TO XI%(KI%) rYI%(KI%) ; GOTO- 3 
YI%(KI%) = YI%(KI%) + 1000 
NS% = 1: GOTO 3040 
10% = IC% + 1 : GOTO 
IF IC% < = 1 THEN 

10% = IC% - 1 : GOTO 
00% = 00% + 1 :NS% = 

G0SU8 3980; IF KE% 

IF KE% =10 THEN 
IF KE% = 11 
IF l<E% = 82 
XBCOC%);YJ = 

GOTO 3420 

EC% = EC% + 1sNS% = 1;XE<EC%) = XHsYE(EC%) - YH _ 

GOSUB 3980= IF KEZ = 67 THEN NSX = 0=ECX = - 1= GOTO 3040 

IF ECX ) -1 THEN HCOLOR= 2: HPLOT XE<ECX - 1),YE(ECX - 1> TO XEIECX 

),YE(EC%); HCOLOR= 3 
GOTO 3045 

‘^G0SUB^3980:^irKE% = 67 THEN NS% = 0!NL% =^NL% - 1: . ^^PLOT 

IF NL% >' 0 THEN AA<NL%) = SQR (CXM - XH) '' 2 + <YM - YH) 2). HPLOT 

XM,YM TO XH,YHsXM = XHsYH = YH 
GOTO 3045 

yll 'l- inix<^?v . ,> < 1000 THEN HCOLOR=. 0= HPLOT XIXCXIX 

! iL?K<KIxL 1) ™ xi^KIX,. INT ((YIX(KIX) / 1000 - INT <YIX(KI 
%) / 1000)) * 1000 + .05): HCOLOR= 3 

KEZ = PEEK < ”• 16384) s IF KE% < 128 THEN 3980 
POKE - 16368, 0sKE% = KEZ - 128: RETURN 

REM stor© input d^ts. . , 

TEXT : HOME :D0* =* CHR^ (4): VTAB 6sA$ =* "Where do you want to sto 

re the Niata ?"s QOSUB 9000 /ct*\ - n twfm 

VTAB 9? HTA8 10| INPUT , "FILE NAME ==>,,. "sFI$s IF LEN (FI*) - 0; THEN 

8020 


THEN 
THEN 
YB<OC’, 


3040 
3040 
3040 

1 :Xa (0C%) 
= 67 THEN 
GOSUB 
GOSUB 3310 
RS%(OC%) ■ = 

) : GOSUB 2 


= XH:YB(OC%) = YH 
NS% = 0:0C% = 0C% 

3420 
GOTO 3420 

A8S (YH - YB(OC%))sRJ% 
/60: GOTO 3040 


3270: GOTO 


1 : GOTO 3040 


= RS%(OC%) sXJ = 




AND 


■- IF ^ LEN ■ ; 4,) 


C RIGHT* (FI*,4T ^ %DAT- OR RIGHt^ 


„ro* 


$. " . _ 

'fCi: 



Mfkl 


'tEFt* 

1: 


(FI*, LEN 


eEl*,4) * 





8080 


8090 

8200 

8210 

8220 

8230 


8240 

8990 

9000 

9010 

9500 

9510 


JJ = 1 TO EC%s PRINT XE< JJ) ? " , " ; YE ( JJ ) 

I KEXT JJ: PRINT OCXs FOR JJ = 1 TO 00%: PRINT XB ( JJ) ? " , " ; YB ( JJ) ? " . " 

?RS%(JJ)s NEXT JJ 

PRINT, NLX: FOR JJ = 1 TO NL%: PRINT AA(JJ)l! NEXT JJ: PRINT D0$i!''CL0 
SE^fFO^: RETURN . 

REM store the picture on screen 
IF KI% < =0 THEN RETURN 

D0$ = CHR$ (4):F0$ = FI^ + ".PIC" 

PRINT D0$: PRINT DO$"OPEN" jrFO$: PRINT DOli; "WRITE" :FO$s PRINT KI%s FOR 
KK = 1 TO KI%: PRINT XI%( KK ) j '% " ? YI%(KI< ) : NEXT KK: PRINT DOf- j "CLOSE" 
;F0$ 

RETURN 

REM Centre string A$ 

B = I NT ( 20 - < LEN ( A$ ) / 2 ) ) : IF B == <0 THEN B = 1 

HTAB- B: PRINT Al:-: RETURN 
REM read stored data 

D0$ = CHR$ (4): TEXT : HOME : VTAB 5:A!fc = "Where is the data stored 
?": GOSUB 9000 

VTAB 8: HTAB 10: INPUT "FILE NAME ==> ":FI$e IF LEN (FI$) = 0 THEN 


9525 IF LEN (FISi) > 4 AND ( RIGHTS (Fl$r4) = ".DAT" OR RIGHTS^ CFI1>,4) = 
".NET" OR RIGHTS (FIS, 4) = ".FIC") THEN FIS = LEFTS (FIS, LEN (FIS 
) - 4) 

9530 FOS = FIS + ".DAT" 

9540 PRINT DOS: PRINT DOS,* "READ" jFOS : INPUT XG,Y0,EC%: FOR KK = 1 TO EC% 

: INPUT XE(KK ) ,YE(KK ) : NEXT KK : INPUT OCX: FOR KK = 1 TO OCX: INPUT 
XB(KK),YB(KK),RSX(KK): NEXT KK 

9550 INPUT NLX: FOR KK = 1 TO NLX: :k:"UT AA(KK:: NEXT KK: PRINT DOS?"CLC 
SE"?FOS 

9560 RETURN 

10000 REM order the obstacles 

10005 FOR JJ -• 1 TO OCX:DX. = XB(JJ) - X0:DY = YB(JJ) - YO: GOSUB 10500:A 
N(JJ) - SP: NEXT JJ 

10010 FOR KK = (OCX - 1) TO 1 STEP - 1: FOR JJ = 1 TO KK 

10C15 IF AN(JJ) < AN(JJ + 1) THEN TE = AN( JJ ) : AN ( JJ ) = AN ( J J + 1 ) : AN ( J J + 
1) = TE:TE = XE ( JJ) :XB ( JJ) = XB(JJ + 1):XB(JJ + 1 ) = TE:TE = YB(JJ): 
YB(JJ) = YB(JJ + 1):YB(JJ + 1) = TE:TE = RSX( JJ) :RSX( JJ) = RSX(JJ 
1 ) :RS%( JJ + 1 ) = TE 

10020 NEXT JJ,KK: RETURN 


10030 REM ass ion distances 

10040 FOR KK = 1 TO OCX:RD(KK) = SQR ((XB(!<K) - XO) (XB(KK) ~ XO) + ( 
YB(KI<) - YO) « (YB(KI<) - YO)): NEXT KK: RETURN 

10500 REM gives theta from vector 

10510 SP = ATN (DY / DX): IF DX < 0 THEN SP = PI + SP 

10520 RETURN 

10550 REM find convex polygon 

10560 2A = XB(1):ZB = XB(1): FOR KK = 2 TO OCX: IF ZA < XB(KK) THEN ZA = 
XB(KK) 

10570 IF XB(KK) < ZB THEN ZB = XB<KK) 

10580 NEXT KK;ZC = YB(1):ZD = YB(1): FOR KK = 2 TO OCX: IF YB(KK) > ZC THEN 
ZC = YB(KK) 

10590 IF YB(KK) < ZD THEN ZD = YBCKK) 

10600 NEXT KK:XC = (ZA + ZB) / 2:YC = (ZC + ZD) / 2:RR = ZA - XC: IF (ZC 
- YC) > RR THEN RF: = (ZC - YC! . 

10605 PRINTi': PRINT : HTAB 6:A4 = 'Tupe in the starting angle"; GOSUf '-D 
OOsAS = "for, c ircular scan."; .GtSUB 9000: PFINT : HTAB 7; INPUT "Sta 
rting angle' in dec. = ";AO:AO = AG / BB 

10610 RR = RR * 1.414:F3X " 0:S3X(P3X/, =-■' Q: FOR K.( 1 TO 2 OCXsZT 

+ PI M (KK - 1) / 0CX:Z1 - XC + RR « = COS (ZT):Z2 = YC RR « 

( 21 ) ' ' 


=■ Ai. 
£ I N 



10620 ZE = CX3(1) - Z1 > * (X3M) - ZD + CY3(1) 
1: FOR JJ = 1 TO OC%;ZF - CX3(JJ) - Z1 ) ^ 
Z2) * (YB<JJ) - Z2): IF 2F < ZE THEN ZE = 


- 22) * 
CXB<JJ> - 

7~.7T,y = 


(Y3 (1 ) 
Z1 > 4- 


- Z2):ZGZ = 
CYBCJJ) - 


10630 
10640 
1065G 
1 0660 
10700 
1071G 
10720 
10750 


IF 
IF 
TR% 
JJ = 
10760 IF 


NEXT JJ: 
P3% = P3% 
NEXT KK; 
RETURN 
REM Form 
OCX - 
OCX - 
= OCX 
Q:P4X 
(IX + 


IF S3XCP3%) 
+ 1:S3X< 

IF S3X(? 


THEN 10650 


3%) =• zg; 


X) = EOXM) THEN P3X = 


0750 

:3oo 


’EN 


tr i angl 25 
P3Z = 0 THEN 
P3Z = 1 THEN 
- 2: IZ = 1 : 

= P3% 

2) < = P4Z 

(34Z(I% + 1)) + STR$ 

J) = S4X(IX>:IX = 1% + 2: 0 
10765 IF P4% = 1% THEN 10730 
10770 IF <P4% - IZ = 1) AND <P4Z 
IZ) ) + STR$ <S4Z( IZ + 1 . ; i- 
0$sS4%(JJ) = S4Z(IZ>: GOT: 10730 
10775 JJ = JJ + 1:TQ$ = STRi <S4Z(IZ)) 
Z(0)): GOSUB 10?30:TR$< JJ) 


FOR KK = 1 TO P3Z:S4Z(KX> = 33Z<;<;<>: NEXT KK: 


JJ - 
IZ + 
OTO ■ 


JJ + 1:T<1$ = STR$ <S4Z(IZ>) + SIRS 
2)): GOSUB 10930:TR$(JJ) = T0$!S4Z<J 
0760 


> 2) THEN JJ = JJ + 1 
STR$ CS4Z{1)>: GOSUB 


STR!& 


TO^ = 
1Q93Q: 


STR$ (34 
TR-^C JJ) = 


ZC 

T 


10780 IF JJ < TRZ 
10790 GOTO 10890 
10800 KK = OsSUZ = 
10810 KK = KK + 1: 
10820 SUZ = 0: FOR 
10830 NEXT JJ 
10840 IF KK > 
10850 


THEN S4Z<0) = 
1 

IF (KK > OCZ) 
JJ = 1 TO P3Z; 
GOTO 1C310 
OCZ THEN 10750 


<S4ZCIZ 
T0$;S4Z(JJ) = S4Z(I%> 
34Z(P4Z):P4% = JJsIZ = 


+ 1) ) + STR$ (34 


1 : GOTO 10760 


OR (SUZ = 0) 
IF 33% (JJ) = 


THEN 10840 
^ KK THEN SUZ 


= 1 


WH^ = 3TR$ (KK - 1) 

$■ = STR$ <S3Z(II) ) + 

= TO$s NEXT II 

10860 JJ = JJ + 1:TQ^ = STR^ 


JJ = Q: FOR II = 1 TO (P3Z - DsJJ = JJ + 1:TQ 
WHf f STR$ (S3Z(II + D): GOSUB 1 0930 : TR* ( J J > 


(S3Z(P3Z)) + UH* + STR$ (S3Z(1))! GOSUB 10 
JJ: GOTO 10890 


930sTR$(JJ) = TQ*:TRZ 
10890 RETURN 
10900 REM verify edges 

10910 IF UAL < LEFT* (EQ*,1)) > UAL ( RIGHT* (EQ*,1)) THEN EO* 
(E<3*,1) + LEFT* (E0*,1> 

10920 RETURN 

10930 REM verify triangle 
10940 FOR KK = 0 TO 2:TLZ(KK) 

TL%(0) > TLZd) THEN TE = 

10950 
10960 
10970 


RIGHT* 


11000 

11010 


= VAL (MID* (TO*,KK +1,1)): NEXT KKs IF 
TLZ(O) :TLZ(0) = TLZ(1):TLZ(D = TE 
IF TLZd) > TLZ(2) THEN TE = TL%(1):TLZd) = TLZ(2) :TLZ(2) = TE 
IF TLZ(0> > TLZd) THEN TE = TLZ(O) :TLZ(0) = TLZd):TLZd) = TE 
TO* = STR* (TLZ(O)) + STR* (TLZd)) + STR* (TLZ(2))s RETURN 
REM identify edges and assign pointers 
UE$<0> = MID* (WT*,1 ,2) :WE*d ) = MID* (WT*,2,2) sWE*C2) = LEFT* ( 


WT*,1) + RIGHT* (UT*,D: FOR JJ = 0 TO 2:KK = 0 
11020 KK = KK + 1s IF KK > EDZ THEN 11050 
11030 IF ED*(KK) = WE*(JJ> THEN 11060 
11040 GOTO 11020 

1:1 Q5Q EDZ = EDZ + 1!ED*(KK) = WE*(JJ)! GOSUB 11600 
11060 WJZ(JJ) » KKs NEXT JJ 

11070 TE = MAL ( LEFT* (UT*,l))5Xi = XB(TE):Y1 = 

1x2 = xb(te)3Y2 ^...ybctejs'te mal 

.X8<tg)''aY3- YB<TE).'r.,„.. . : 

11080 FOR JJ » b TO 28X4 = HXCWJZ( JJ));Y4 =HY(WJZ(JJ) )s GOSUB 261508WIZ 

(JJ) = SUZs" ^NEXT JJ- 

11090 IF WIZ<0) > UlZd) THEN TE » WIZ(Q) sWIZCO) = WIZCDsUIZd) = TEsTE 
=» WJZ<0)5WJ%(0) = WJZd)aWJXC1).3. TE ^ 

11100 IF WIZd) > WIZ<2) THEN TE * »lZtf).:WIZd ) » WIZC2):WIZ<2) » TEaTE 
» WJZd):UJZd) » WJZ(2>aWJX<2>| » ,Ti;^ 


YB(TE)sTE = VAL < MID* 
C RIGHT* (WT*d))sX3 = 


■11-110 IF WIX<0> > THEN TE = UI%<0) 5UI%(0> = UI%(1 

= WJX<Q) :WJ/i<0) = yjXd ) ) = TE 

■11120 C1X = Os FOR JJ = 0 TO 2s IF UIX<JJ) = 0 THEN C1X = 
11130 NEXT JJs IF C1% = 2 THEN 11150 

11140 TH%<yj%<1)) = IwSCO) ;TN%(WJ%<1 ) ) = 0 ; TM% ( U J% C 2 ) ) = 
)) = 0: GOTO 11-160 


yiXCD = TEsTE 

X-;0> :TNX<yj%<2 


11150 THX(UJ%<2)) = Ww'MO) :TNX(WJ%(2) ) = UJ%<1) 

11160 RETURN 

11600 REH fill in head, tail and mid-points 

11610 yi% = VAL ( LEFT^ ( E0$< KK ) ,1 ) ) : V2X = VAL ( RIGHTS ' El $ ( XK > , 1 > ) : IF 
XB<U2%) = X8CV1%) THEN H = 9.999E17: GOTO 1-l’620 
11615 H = (YB<V2%> - Y3(yiX)) / (XB(V2%> - XB(yi%)) 

11620 H1 = RS%(yi%) / SOR (-t ■+■ H » M):X1 = XB(V1%) + 3GN (X3CV2%> - XB < 
yi%)) » H1:Y1 = n * (X1 - XB<V1%)> + Y3(V1%)s«2 = RE'::,2%> / SQR (1 
N » H)5X2 = XB<V2%) SGN (X8<V1%). - XB(V2X)) * M2:V2 = YB<V2%) 


M * (X2 - XB(V2%>):X3 = <X1 + X2) / 2sY3 = <Y1 + Y2> ' 2 

11625 EXCXK) = X3sEY(KK) = Y3 

11630 X4 = XB(V1%) + H * H1:Y4 = YBCVIX) - CX4 - XB(V1X)) / H:X5 = XBCVEX 
) ■+• H * M2:Y5 = YB(V2%) - (X5 - XBCM2%)) / HsM3 = <Y5 - Y4> / CX5 - 
X4):HX<KK) = - (Y4 - Y3 - X3 / H - H3 » X4) / <1 / N + M3):HY(KK) = 

Y3 - (HX(KK) - X3) / fi 

11640 X4 = X8CU1X) - H * M1:Y4 = Y8<V1%> - (X4 - XBCOIX)) / HsX5 = XB(y2% 

) - M » M25Y5 •■= YBCVOX) - CX5 - XB(V2%>) / N:M4 = <Y5 - Y4) / (X5 - 
X4)sTX<KK) = - <Y4 - Y3 - X3 / M - M4 » X4) / (1 / H ■+• M4)sTY<KK> = 

Y3 - (TX<KK> - X3) / H 

11650 IF (XQ - HX(KK>) '' 2 + <Y0 - HY(KK>) '' 2 > <X0 - TX(KK)) '' 2 + <Y0 

- TY<KK)) 2 THEN TE = HX < KK ) s HX ( KK ) = TX < XK ) s TX ( KK) = TE s TE = HYC 

KK):HY<KK) = TY(KK) :TY<KK ) = TE 

11660 RETURN 

26150 REM point inside triangle ? 

26160 A = <Y4 - Y1):B = (X1 - X4);C = - B Y1 - A * X1:L1 = A * X3 B * 

Y3 ■+• CsL2 = A » X2 + B * Y2 + C:C1% = Os IF (L1 > 0 AND L2 > 0) OR ( 

L1 < 0 AND L2 < 0) THEN C1% =1 

26170 A = (Y4 - Y2):8 = (X2 - X4):C = - B * y2 - A » X2sL1 = A * X3 + 8 * 

Y3 + C:L2 = A * X1 + B » Y1 + 0:02% = 0: IF (L1 > 0 AND L2 > 0) OR ( 

L1 < 0 AND L2 < 0) THEN C2X = 1 

26180 A = <Y4 - Y3):B = (X3 - X4):C = - 8 » Y3 - A * X3sL1 = A * X1 + 8 * 

Y1 CsL2 = A » X2 + B » Y2 CsC3X = 0: IF (L1 > 0 AND L2 > 0) OR C 

LI < 0 AND L2 < 0> THEN C3% = 1 

26190 IF <01% AND C2X) OR <C2X AND C3%) OR (C1X AND C3%) THEN SU% - 0: RtTUR 


26195 SU% = 1; RETURN 

41000 DIM EX<12) ,EY<12) ,EDf (12) ,HX(12) ,HY(12) ,TX(12) ,TY(12) ,TM%<1c.) ,TN%( 
12) ,RD(10> ,AN<10) ,TL%(2) ,UE1K2) ,WJX(2) ,WIX<2) 

41010 RETURN 

50000 REM print all information _ ^ 

50010 FOR II = 1 TO OCXs PRINT “XB ( " ; 1 1 ; " ) = "sXBdl);" YB<“;II? ) - ?Y 

BCIDs NEXT II ,, , 

50020 GET KE^i FOR II = 1 TO OCX: PRINT “RSX( “ ; II ; " ) - “;RSX<II)s NEXT I 
I: GET KE*s FOR II = 1 TO EDXs PRINT “ED$( " ? II : “ > = “ ,-ED^< II ) ; " TM%< 
“Jll?") » "jTMXCII) TNXC"! II;") - "jTNXdDs PRINT ''HX( " ; II ; ” ) - 
jHXdDj" HYC-jII?") = “jHYdD 

50030 PRINT "rXC"; II » "jTXdDj" TYt'^jII?") = “jTYdl): PRINT “EX< j 

iiiTV w riexcii)?" EY<'*?ii»'').'= “jEYcrns next ii 

50040 RfeTlJRN'' -wiNit ' ; ^ i. 

50100 REN^ print- vn - - vn-. 
50110 TEXT ■:» MQMEr? in ?X0|, ^ 

r)"* PRINT s HTAB 7s PRINT "Number FOR JJ * 

1 TO EC%s HTAB 7t PRINT "Effst ^ ’X* 

"s NEXT JJ 




pn-1'^n P?IN.T : HTA3 7; PRINT -'Nunibsr. of obstacles = "?0C%; PGR J-J - ‘ ^ 

WAB 3: P R INI " Ob st acl e '' I J J ? " at <"? X3 ( JJ );'%"? T3 ( JJ ) J " > witn 

f ac * -is ^’jSSXCvJJ/ J NeXT JJ _ „ ^ ,, , _ 

=:n-ioQ > □ THEN PRINT : HTAB 7; PRINT "Number of links - jNe.-.:- = 

Q. -OR JJ = 1 TO NL%: HTA8 5s PRINT "Length of link - sAACJJ 

):L = L + AA<JJ): NEXT JJs HTA8 9; PRINT "Total length = "?L: RETURN 

50200 REN plot the points ^ 

502't0 HER 2 s HC0L0R= 3 s' GOSUB 503QQ 

50220 FOR JJ = 1 TO 0C%: GOSUB 50340: NEXT JJ 

50290 RETURN 

273" ™ S7?,iS = "hpCSt 379,3 TO 373,10= HPLOT 3,18. TO 7,188 

50320*' Kr^JJ^'^^TO to 270^STEP 10: HPLOT JJ,0 TO JJ.Si NEXT : FOR JJ - 10 
TO 130 STEP 10: HPLOT 0,JJ TO 3,JJ; NEXT 

FOR^KK = 0 TO 360 STEP 10:X1 = XB(JJ) + RS%<JJ> * COS <KK / BSJjY 
503-0^ FOR^KK^^ 0 ^^^60 ^ STEP^^^ ^ XB < JJ) , VB ( J J) TO X1,Y1 = 

NEXT KKs RETURN 



1 REM 


2 

REM 




3 

REM 


OBSTACLE AVOIDANCE 

. ^ 


REM 

* 

PROGRAM 


5 

REM 




6 

REM 
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7 

REM 
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REM 





9 REM 

20 DIM EDf (10) ,XHC3Q),YHC30> ,X<9,5) ,Y(9,5),XE(15’) ,¥E<15).XB<6) ,YB<6> ,S1% 
(6) ,S3):<5) ,S4%(5) ,LH(5),EXC10> ,EY<10) ,HX(10) , riY ( 1 0) , TX( 1 0 ) . TY < 1 0 ) , RS 
X(7) ,AN(7) .RD(7> ,L2%(3>,TMX(ia) ,TNX( 10 > , AA(6) ,XF<15) VYF< 15) ,MKX<6) 

50 PI =3.14159265:88 = 180 / PI:EP% = 10 

105 G0SU8 2100:RD(0CZ + 1) = 1E20:RD(0) = 1E20 

110 G03U8 60000: POKE 216,0 

150 HOME : VTAB 10: FOR KK = 1 TO NL%: PRINT ‘'Linsit of Joint ":KKr' = ; 

: INPUT JL(KK) : JL<KK> = JL(KK) / 88: NEXT KK: GOTO 240 
160 FOR KK = 1 TO P3% - 1: HPLOT XB (S3%<KK ) ) , YB ( S3X ( KK > ) T0X8(S3%<KK+ 

1 ) ) , YB<S3X<KK + D): NEXT KK: HPLOT XB ( S3%<P3Z) ) , YB ( S3% (P3%) ) TO X8 ( 
S3%<1 ) ) ,YB <S3%(1 ) ) 

240 GG% = 1;PF% = 0:EI% = 0:EH% = 1;NP% = 5:NSX = 5:U2% = - 1:H% =0 

250 G0SU8 24000: GOSUB 28000 
255 GOTO 250 

260 GOSUB 50200: FOR HH = 1 TO ED%; HPLOT TX(HH) ,TY<HH) : GET KEfs HPLOT 
TX(HH),TYCHH) TO HX<HH> ,HY<HH) : NEXT HH 
1000 END 

2100 REM read network from disk 

2110 TEXT ; HOME ; VTAB 6: PRINT "Uhich file do you want to operate upon 

2120 VTAB 9: HTAB 10; INPUT "FILE NAME ==> ";FI$: IF LEN <FI$) = 0 THEN 
21 20 

2130 IF LEN (FI$) > 4 AND C RIGHT* (FI*, 4) = ".DAT" OR RIGHT* (FI*,4> = 
".NET" OR RIGHT* (FI*,4> = ",PIC" OR RIGHT* (FI*, 4) = '.JNT") THEN 
FI* = LEFT* (FI*, LEN (FI*) - 4> 

2140 FO* = FI* + ".NET" 

2150 DO* = CHR* (4): PRINT DO*: PRINT DO*? "READ” ;F0* 

2160 INPUT Xa,YO,EC%: FOR JJ =1 TO ECZ: INPUT XE C JJ) , YE{ JJ) : NEXT JJ 
2170 INPUT 0C%: FOR JJ = 1 TO 0C%: INPUT XB( JJ) ,YB( JJ) ,RSZ( JJ) ,RD( JJ> , AN 
XJJ): NEXT JJ; INPUT NL%;L = 0; FOR JJ = 1 TO NL%: INPUT AA(JJ);L = 

L + AA(JJ>: NEXT JJ 

2180 INPUT ED%: FOR JJ = 1 TO ED%s INPUT ED*( JJ) ,TM%( JJ) ,TN%( JJ) , HX( JJ) , 
HY(JJ),TX:(JJ),TY(JJ),EX(JJ),EY<JJ): NEXT JJ 
2190 PRINT DO*?"CLOSE"?FO*s RETURN 
10500 REM gives slope In -90 to +270 range 
10510 IF DX = 0 THEN SP = PI / 2: GOTO 10530 
10520 SP = ATN (DY / DX) 

10530 IF OX < 0 THEN SP = SP + PI 
10540 RETURN 

10600 REM gives slope in Ouad 1 & 4 

10610 IF DX = 0 THEN SP = PI / 2a GOTO 10630 

10620 SP » ATN (DY / DX) 

10630 RETURN • :: 

20000 REM find manipulator 3t;ate 

20010 DX = (XP XO);DY = (YP - Y0> a GOSUB 10500- 

20015 IF SP > * AN<1>‘ -THEN-NTK-' 



20018 IF SP < AM(0C%) THEM NTX = 00% + 1: RETURN 
20020 NT% = 2 

20025 IF SP < AN<NT% - 1) AND SP > = AN(NT%) THEN RETURN 

20028 NT% = NT% + 1 : GOTO 20025 
20030 RETURN 

2GG50 REii f ind index of reqd. edge 
2G060 J% =0 

20070 J% = J% + 1: IF <ED$<J%) = EOt) OR <J% > ED%) THEN 20090 
20030 GOTO 20070 

2Q09Q IF 1% > ED% THEN ED% = ED% + 1:KK = ED%:ED$<KK> = EOS: GOSUS 11600 


2QQ95 RETURN 

201GQ REii assign markers fco. edges 

20110 FOR JJ = 0 TO P1%:HK%(JJ) = 0: NEXT JJ:X4 = X0:Y4 = YO;JJ = P1:::?4 
% = 0 

20130 IF JJ = - 1 THEN 20190 

20140 GOSUB 20300:SU% = 0 

20150 II = II + 1: IF (II > P4%> OR (SUX = 1 ) THEN 20170 

20160 X3 = XB(S4%(II) ) :Y3 = YS ( S4X( II ) ) :R1 = SS%CS4% ( II > ) : GOSUB 26000: GOTO 
20150 

20170 IF SU% = 1 THEN MK%<JJ + 1) = 1:X4 = EX(JJ + 1):Y4 = EY(JJ +1):P4 
% - 0: JJ = JJ + 1 
20180 JJ = JJ - 1: GOTO 20130 • 

20190 RETURN 

20300 REM update obstacle set 

20310 01% = UAL ( LEFTS ( EDS ( SI X < JJ ) ) , 1 ) ) s V2X = UAL C RIGHTS <EDS(S1X(J ; 

J>>,1>):II = 0 i 

20320 II = II + 1: IF (II > P4%) OR (S4X(II) = V1X) THEN 2G350 ' I 

20340 GOTO 20320 

20350 IF II > P4X THEN P4% = P4X + 1!S4%(P4X) = 01% | 

20360 II = 0 i 

20370 II = II +1: IF (II > P4%) OR (S4%(II> = 02%) THEN 20390 
20380 GOTO 20370 

20390 IF II > P4X THEN P4X = P4% + 1:S4%(P4%) = V2X 

20395 RETURN . - i 

20500 REM find Bezier points within network j 

20510 KK = 0:02% = D2X + 1 :Di% = 0!X(D1%,D2%) = X(L2%(D2%> rD2% - i):Y(D1% ! 

.02%) = Y<L2%(D2%) ,02% - t>:D1% = D1% + 1:X(D1%,02%> = HXCS1%(KK > > : Y i 
(01%,D2%) = HY(S1X(KK)> I 

20520 KK = KK + 1: IF KK > P1% THEN 20600 ! 


20530 IF 1 = MK%(KK) THEN 20560 
20535 IF P1% < KK + 1 THEN 20560 

20537 V1% = VAL ( LEFTS (EDS(S1%(KK) ) ,1 ) ) :U2X = VAL ( RIGHTS (EDS<S1X(K 
K)),1)):DX = XB(VIX) - XB(V2X):DY = YB(VIX) - Y8CV2%): GOSUB 10600:M 
1 = SPsDX = X(D1X,D2%) - EX(S1%(KK)>:DY = Y(D1%,D2%) - EY(S1X(KK)) 


20540 


20545 


20550 

20560 

20570 

20580 

20590 


GOSUB 106a0!M1 = M1 - SPsLJ = (EX(S1X(KK>) - EX(S1%(KK + 1>)) ^ 2 + 
(EY(S1%(KK)) - EY(S1%(KK + 1))) 2:M = (EY(S1%(KK)> - EY(S1%(KK + 1 

))) / (EX(S1X(KK)> - EX(31%(KK + 1))) 

D1% = D1% + 1:X(D1%,D2X) = EX(S1X(KK)) + SGN (EX(S1%(KK + 1)> - EX 
(SIX(KK))) » ABS ( COS (MD) » SOR (LJ / (1 + M » M) > s Y(D1%,D2X) = 

M (X(D1%,D2X) - EX(S1%<KK))> + EY(S1X(KK)) 

KK == KK + 1s IF KK > P1% THEN 20600 
IF MK%(KK> = 1 THEN 20580 

D1X = D1% + 1sX(D1X,D2X) = EX(S1%('KK > ) : Y(D1X,D2X) = EY<S1%(KK))s GOTO 
20550 . 

D1% = D1% + 1:X(D1%,D2%) = TX(S1%(KK ) ) :Y(D1%,D2%) = TY(S1%(KK ) ) :D1% 

= D1% + 1:X(D1%,D2%) « EX<S1X(KK ) ) sY(D1X,D2X) = EY(S1X(KK)) 

L2%(D2%) = D1%sD2% « 02% + 1 -01% * 0aX(01%,D2%) J* EXCSIXCKK ) ) sY<D1% 
,02%) » EY<S1X<KK))801% * 01% + 15X<013^y02%)W» HX(S1%(KK))5Y<01%,D2% 

) » HY<S1%<KK>)s GOTO 20550 ^ 



20600 L2%(D2%) = D1%; RETUyr4 
20700 REH find nearest obstacle . 

20710 yi% = VAL C LEFTf ( E- * C ’K ? 1 % > ) ,1 ) ) : U2% = VAL 
P1%)>,1)): IF Y3(V1%) = Y3(V2%> THEH H =~?.?99E't7: 
20715 M = (XB<V1%) - X3CV2*;: 5 / CY3<V2%) - Y3<V1”D ) 

20720 XUZ = EX(S1%(?1/1)) + E3n (XO - EX'< 31 i? 1 X ) ) i a L 
):YU% = <i1 * (XUX ~ EX ■: z'^ZCP 1Z) ) ) + EY < 31 Y i ?■’. .*1 ) ) 
2Q725 XWrC = XO - L / 2:YUZ - YD / 2 
20730 RETURN 


( RIGHT$ <EB'^<Si:i( 
GOTO 20720 

/ 3'3R (1 + M » H 

Y0> / 2 


2CdOO REH solve dill ema w ; t n two poi ntsrs 

20S10 IF (XQ - EX(TH%C J%) ) '' 2 + <Y0 - EYCTHX(J%))) '' 2 < CXO - EXdNlK 

j%))) 2 + <Y0 - ey<~'--:>:j%))) 2 then pix = ?1% + issi%(PiX) = tmx 

= TMXCJX); RETURN 

20820 P1% = P1X + 1:S1X(Pi:i> = TN%<JX):J% = TNX<J%): RETURN 
2 I 0 OO REH calculates max i au.n amplitude 

21510 WACO) = 0:WACL2XCKK> ) = 1:A = 0: FOR Z2 = 1 TO L2%CKK):A = A + SOR 
(CXCZZrKK) - XCZZ - 2 + CYCZZ,KK) - YCZZ - 1 , KX ) ) '"' 2 > ; NEXT 

zz 

21520 B = Q: FOR ZZ = 1 TO L2%CKK) - 1:8 = B + SQR CCX(ZZ,XK) - XCZZ - 1 
,KK>) 2 + <Y<ZZ,KK) - YCZZ - 1,Ki<)) 2):UAC2Z> = B / A: NEXT ZZ 

21530 G = 0: FOR ZZ = 1 TO L2%CKK): IF ABS CZZ / L2%CKK) - UACZZ)) > ABS 
CC) THEN C = ZZ / L2XCKK) - WACZZ) 

21540 NEXT ZZ: PRINT "Max i mum ampl i tude = "jC: RETURN 
23000 REH Find correction factor 

23010 PL = .5 » C * SIN CV * PI): PRINT "Correction factor = "ssPL: RETURN 


24000 REM find next end-effector position 

24010 IF EI% > 0 THEN 241 GO ' , 

24015 IF PEEK C - 16384) >127 THEN PRINT : PRINT CHR‘.& C 7 ) ? " Jump i ng t 
o next input point on request ":GGZ = PF%: POKE -16368,0 
24020 IF GG% < PF% THEN GG% = GGX + 1:XP = XFCGe%):YP = YFCGGX)^ RETURN 


YECEHX + 1)) 


24030 IF EH% > = EGX THEN PRINT D0$; "CLOSE" ;F0f: END 

24040 PF% = SOR CCXECEHX) - XECEH'Z + D) 2 + CVECEHX) 

2) / 10: IF (XECEHZ) - XECEHZ ^ 1)> < ) 0 THEN 24055 
24045 FOR KK = 1 TO PF%:XFCXK) = XECEHX) :YFCKX > = YE(EHX) + SGN CYECEHX 
+ 1) - YECEHZ)) » KK ♦ 10: NEXT KK:XFCQ) = XECEHZ) : YFCO) = YECEHZ): 
GOTO 24080 

24055 M4 = (YECEHZ) ~ YECEHZ + 1)) / CXECEHZ) - XECEHZ + 1)> 

24060 FOR KK = 1 TO PFZsXFCKK) = XECEHZ) + SGN (XECEHZ + 1) - XECEHZ)) * 

10 » KK / SQR C1 + M4 « M4>:YFCKK) = M4 * CXFCKK) - XECEHZ)) + YECE 
HZ)s NEXT KK:XFCO> = XECEHZ) :YF(0) = YECEHZ) 

24080 EH% = EHZ + 1:GGZ = - Is GOTO 24020 

24100 IF EIZ = 2 THEN E6Z = EGZ + 1 s IF E6% > PGZ THEN EIZ = Os PRINT CHR^ 

<7)s PRINT "State change operation is over.": PRINT "Resuming operat 
ional path now.": PRINT sGGZ = GGZ - 1 : GOTO 24010 
24110 IF EIZ = 2 THEN XP = XHCEG%);YP = YHCEGZ); RETURN 

24120 01 = XPs02 = YP;Q3 = XP:04 = YP:S4ZC1) = NPZ - 1sP4Z = 1:XC = CQ1 + 
X8(S4%C1))) / 2sYC - CQ2 + Y8(S4Z(1))) / 2: GOSUB 24500;EGZ = OsEIZ = 

2s GOTO 24100 

REM tells theta from vector 
IF DX = Q THEN ;SP = PI / 2s GOTO 24485 

SP = ATN (DY / DX): IF DX > 0 AND DY > =0 THEN RETURN 

IF DX < 0, THEN SP * SP + PI: RETURN 

IF DX > 0 AND.DY < 0 THEN SP » 2 # PI + SP 

return ' , : . , 

REM how ,to-^ change state- ^ /•u; 

PRINT CHRf (7) a, PRINT "Go I for a STAiiTE CHANGE now.": PRINT "App 
reaching obstacle "5S4X<f 43Cl 
XB;(:i4:|fi:fi;|t 


24470 

24475 

24478 

24480 

24490 

24500 

24505 


24510 PGZ 
M1 » 


It 


OsDX r* <G1 
9.999E17a GOTO 


(02 


YB(S4Z(1 n > a IF DY * 0 THEN 


If' 



YBC 


24520 Hi = - (DX / DY> 

24530 X1 = Xa <S4X(1) > + (RS%(S4%(1 ; ) 4. EPX) / SQR <1 + Ml * 

S4%( 1 ) ) + M1 » <X1 - X3 (S4% C - > ; ;> j 

24535 REM X2=XB C S4X ( 1 ))- C R3% < S4% . I ;> -£p% ) /SQR C 1 +M1 +M 1 >: Y2=Y3 C 34% ( '1 )) i-M1 * ! 

(X2-X8 (S4%C 1 ) > > i iF<X2-XC )-'''2+r* 2- fC ) ''‘2> (X1-XC)'' Z’-?'. ’."1 -'■'’O) '''Z''4;£4Xi=X2: Y ' 
1=Y2' . i 

2^^537 X2 = XB<S4%C1)) - < RS% CS4% ( ' : ; - E?%> / 3:.3 ;1 i- Ml 4 M1):Y2 = YBC . 

34%C1)) + Hi 4 (X2 - X3CS4%(1 :■ '> 5': IF Y2 < f> Xt = XZsY^ = Y2 

24540 U1 = sWZ = <32 sW3 = X1 :W4 = Y1 ; G03U3 2'i3a0::i = 1' ^ 

24550 II = II + 1! IF II > P4% THE.X 24650 

24560 DY - YBCS4%(II)) - YB<S4%CII - 1>>:DX = X3^^S4%<:i)) ~ X3C34%CII - 1 
)); IF DY = 0 THEN M1 = 9.999=1:7: GOTO 24530 
24570 Ml = - CDX / DY) 

24580 X2 - X8CS4%<II - i)) + < RS% C 34% C 1 1 - 1 ) ) + EP% ) / SOS ( 1 + stl 4 M1 
) ;Y2 = YB<S4%<II - 1 ) ) + M1 * vX2 - X3CS4%<II - 1 ) ) ) 

24585 X3 = XB(S4%<II - 1)) - CRS%C34%{II - D) + EP%5 / SOR (1 + M1 * M1 

) :Y3 = YBCS4%<II - 1>) + Mi » (X3 - XaCS4%<II - 1 ))> S 

24590 IF (X3 - XC) '' 2 + < Y3 - YC ) '■* 2 > CX2 - XC ) 2 + ( Y2 - YC> 2 THEt^ 

X2 = X3sY2 = Y3 

24600 Wi = Xi:U2 = Y1:U3 = X2:U4 = Y2:W5 = XB(S4%CII - 1>)!W6 = Y3(S4%(II | 

- 1>>;R1 = RS%(S4%CII - i)); GOSUB 24360 j 

24610 X1 = X8<S4%<II)) + C RS% ( 34% < I I ) ) + EPX) / SQR (1 + Ml » H1):Y1 - Y f 
BCS4%CII)) + Hi 4 <X1 - XB<S4%(II))) | 

24615 X3 = XB<S4%(II)) - ( RS% ( 34% ( I I ) ) + EPX) / SOR <1 + M1 » Hi > 5Y3 = Y t 
B<S4%<II)) + M1 * (X3 - XB<S4%(II))) 

24620 IF CX3 - XC) '' 2 + <Y3 - YC) 2 > (XI - XC) '' 2 + CY1 - YC) '' 2 THEN 
XI = X3sY1 = Y3 : . : 

24,630 W1 = X2sU2 = Y2:W3 = X1;W4 = Yl : GOSUB 24SC0 I 

24640 GOTO 24550 

24650 DY = 04 - YB<34%<II - 1)):DX = 03 - XB<S4%CII - 1)): IF DY = 0 THEN ‘ 
Ml = 9-999E17: GOTO 24670 
24660 M1 = - CDX / DY) 

24670 X2 = XBCS4%<II - 1)) - CRS%(S4X(II - 1)) + EPX) / S<3R Cl + Ml * Ml 

):Y2 = Y8CS4%<II - 1)) + Ml * 'YZ - XHCSAXCII - 1))) j 

24675 REM X3=XB < 34%< 1 1-1 ) ) + CRS%<S42< 1 1-1 ) ) +EP% ) /SQR C 1 -f M1 ^M1 ) : Y3=YB <S4%C I 
1-1 ) )+M1»CX3-XBCS4%(II-1 ) ) ) I IF<X3-XC)''‘2+(Y3-YC)*''2>CX2-XC)''2+CY2-YC)'' 
2THEHX2=X3:Y2=Y3 

2468Q X3 = XBCS4%CII - 1)) + (RS%(34Z<II - 1)) +■ EPX) / SOR (1 + M1 it Mi 

):y3 = Y8CS4X(II - D) + Mi * CX3 - XBCS4%(II - 1)))s IF Y3 > Y2 THEN I 


X2 = X3:Y2 = Y3 

24690 W1 = X1:U2 = Y1;W3 = X2:U4 = Y2sW5 = X8CS4%CII - 1)>:W6 = YBCS4%(II 

- 1)):R1 = RSXCS4XCII - 1>)s GOSUB 24860 

24700 W3 = 03sW4 = Q4:W1 = X2:W2 = Y2: GOSUB 248Q0s RETURN 
24800 REM straight line segmentation 

24810 IF (U1 - W3) = 0 THEN M3 = 9.999E17; GOTO 24830 
24820 M3 = CW2 - W4) / CWi - W3) 

24830 NX = SOR < CU1 - U3) '' 2 + CU2 - «4) 2) / 10: FOR KX = 1 TO N%:PG 

X = PGX + IsXHCPGX) = W1 + SGN (W3 - W1 ) Jt 10 » KK / SOR Cl + M3 » 

M3):YH(PGX) = -H3~3t "fXRCPGX) - W17 + W2: NEXT KIC 

24840 RETURN r 

24860 REMicircular: segmentat ion 5 

24870 DX » <«I1 4>y5>:E>Y * (W2 - W6)s GOSUB 10500sA = SP + 4 » PIsDX = (U3 

- W5)sDY /= (W4 - W6)! GOSUB 10500:B = SP + 4 » PI 

24875 PRINT "A- « deg": PRINT "B = "j INT CB * BB)?" de 

24880 NN = ( ABS (B - A>),» (Ri + EPX) / lOsPD = 10 / (R1 + EPX): FOR KK =? 
1 TO NNsPGX » PGX^'* ikXHCPGXi:* CR1 + EPX) » COS (A - KK » PD) + U5 
sYHCPGX) =» (R1; + EPX>^ * KK » PD> + M6s NEXT KK 

24890 RETURN ' - 



SSR 


25000 REM val idate set 

25Q15 OF = SOR ( CXO - XP ) ''' 2 + <Y0 - YP ) 2): FOR KK = 1 TO OCX: CD = 

((XO - XB<XK)) 2 + CYQ - Y8(KK)) 2)s IF OD - RS%(KK) > = L THEN 

"•A'KXK) = G: GOTO 2503Q ' '■ 

25CZC OE = 30R < (XP - XB<KK>) '"2 + (YQ - YB (KK ) ) 2 ) : IF OD + OE - 2 * 

R3X<KK) > = L THEN TA%<KK> = 0; GOTO 25030 
25G25 TAX(i<K ) = 1 ' 

25030 3C3U3 25050: NEXT KK: RETURN 

25050 REii eliminate all beyond E 
25060 IF DT(KK) > OF + RSX(KK) THEN TA%(KK) = 0 
25070 RETURN ‘ 

25490 IF (X3 - XC) '' 2 + (Y3 - YC) 2 > (X2 - XC) 2 + {Y2 - YC) 2 THEN 
X2 = X3;Y2 = Y3 


26000 REH line cuts circle ? 

26010 IF (<X1 - X3) 2 + (Y1 - Y3> 2) < R1 * R1 OR ( (X2 - X3> ''' 2 + < 

Y2 - Y3) 2) < R1 » R1 THEN SU% = 1: RETURN 

26020 M = (Y2 - Y1 ) / <X2 - X1>:X4 = (Y3 - Y1 + M * X1 + X3 / M) / (N + 1 
/ m>;Y4 = Y1 + M » (X4 - X1>:PD = <X3 - X4> 2 + (Y3 - Y4) '' 2 

26030 IF PD > R1 » R1 THEN SUX = 0: RETURN 

26040 DX = X1 - X3:DY = Y1 - Y3: GOSUB 24470:M1 = SP:DX = X2 - X3:DY = Y2 
- Y3: GOSUB 24470:H2 = ABS <H1 - SP > : IF fi2 > PI THEN M2 = 2 » PI - 


H2 

26050 IF M2 < PI / 2 THEN SUX = 0: RETURN 

26060 SUX = 1: RETURN 

26150 REM point inside triangle ? 

26160 A = <Y4 - Y1):B = (X1 - X4):G = - B » Y1 - A » X1:L1 = A * X3 + B » 

Y3 + C:L2 = A X2 + B * Y2 + C:C1X = 0: IF <L1 > 0 AND L2 > 0) OR ( ' 

Li < 0 AND L2 < 0) THEN C1X =1 

26170 A = (Y4 - Y2>:B = (X2 - X4):C == - B « Y2 - A » X2:L1 = A » X3 + B » 

Y3 + C:L2 = A * X1 + B * Y1 + C:C2X = 0: IF <L1 > 0 AND L2 > Q> OR ( 

LI < 0 AND L2 < 0> THEN C2X = 1 

26180 A = (Y4 - Y3):B = <X3 - X4):C = - 8 * Y3 - A * X3:L1 = A » X1 + B » 

Y1 + C:L2 = A * X2 + B » Y2 + C:C3X = 0; IF (Li > 0 AND L2 > 0) OR ( 

Li < 0 AND L2 < 0) THEN C3X = 1 

26190 IF (C1% AND C2X) OR (C2% AND C3X) OR (C1X AND C3%) THEN SUX = 0: RETUR 


26195 SUX = 1: RETURN 

28000 REM solves manipulator configuration 

28005 HX = HX + 1;CX = isFX = 0: PRINT 5 HTAB 10: PRINT "Iteration number 

";HX; PRINT : IF SOR ( (XP - XQ) 2 + (YP - YO) ''2) > L THEN PRINT 
"Point (~ INT (XP)"," INT (YP)"> is i naccessab le . " : RETURN 
28010 X1 = XP^YI = YP:X(0,0) = XP!Y(0,0) = YP : GOSUB 20000: PRINT "Nat. St 

ate = ";NTX: GOSUB 28500:NSX = NTX: IF NPX = 1 OR NPX > OCX THEN GOSUB 


35000: RETURN 

28020 NR$ == STR$ (NPX - 1) + STR^ (NPX): PRINT 
NR»: IF NPX ( > NSX THEN GOSUB 30500: GOTO 


28030 XI = 
NSX - 
GOTO 
28040 GOSi 
28050 IF 
28060 FX » 


XI ■= X0:Y1 = Y0:X2 = XB(NSX):Y2 = YB(NSX):X3 
SX - 1):X4 = XP:Y4 = YP : GOSUB 26150: IF SUX 
GOTO 28050 
GOSUB 30000 it 

IF' <li;;«'m”,ANDiFX;%':0)H,-THEH 28200 
FX =» OsCX “ OX f ■f *7 GOSUB 31300sD1X = 0:D2X = 
GOSUB 31000s GOSUB 320005 GOSUB 33000 
IF <II « 0:t AND FX * 0> THEH 28200 , ' ^ 

FX = OsCX = CX + 1: GOSUB 31500sD1X * OsD2X = 


28070 

28080 

9 

28100 

28200 

28SOO 


Present f 
28050 
= X8(NSX 
= 1 THEN 


first edge = 


OsD2X = OsKK 


GOSUB 310005 GOSUB 320001 OOSUB 3^000 
IF <II < >0 OR’ FX-'»,«i'ANOVE|gMi;,O^T 

RETURN ’*■ 

REM t« calculation 


TV I 


IF NTX » NSX THEN J8SI 


- 1 ) :Y3 = YB( 
GOSUB 30200: 


GOSUB 


L, I' ' 


GOSUB 20530 



28520 A = NT%; IF RD(NS%) < RD(A> THEN A = NSX 
28525 PRINT "Crossing a ray at this stage ?RIMT 
te = "?NS%; PRINT "Present state = PRINT 


r ? V < o u 5 
'Nearest 


natural sta 
obst ac 1 e = 


2 + (YO - 
I RETURN 


28550 IF (XO - XP) 

= *1 * U'^y =;■ ' — . -I 

28550 J1%'= o7 RETURN 
28550 IF NSX < > NPX THEN U1% = 

28560 XI = X0:Y1 = Y0:X2 = XBCNPX 


YP) 


0 ; 


2 < RD(A) * RD< 


A) THEN NPX = NTXsUi 


YBCNPX): GOSUB 26150: IF SU% = 


RETURN 
1 ) :Y2 = 
U2% THEN 


YB(NP% 

: i -it y . 


28570 
29000 
2901 0 
29100 


- 1 ) :X3 = 
0: RETURN 


X8CNPX)sY3 = 


1 


1 


RETURN 
THEN FC = 1: 
• 1 TO 2 STEP 


RETURN 
- 1 : FC 


U2X = SUXiUlX = 

IF X = 0 OR X 
FC = X: FOR I = 

REM n C r 

IB 29000 sNC = FC:X = RR: G03U3 2 
29000 :NC = NC / FC; RETURN 
ed Composite Simpson’'s Rule 
TO N:W(UU) = (UU - 1) / (N - 1) 
DY<UU>): NEXT UU;B = 1:A = 
- 1 ) 


FC » I: NEXT I: RETURN 


29110 

X = 

NN: 60S 


- RR 

: GOSUB 

29120 

REM 

Mod i f 

29130 

FOR 

UU = 1 

DX(UU 

) + DY( 

29140 

H = 

(B - A) 

29150 

N2 = 

N - 2: 

29160 

FOR 

1 = 2 

29170 

10 = 

1-1: 

29180 

B1 = 

(F(I) 

29190 

82 = 

(F(I1 ) 

29200 

B3 = 

(F(I2) 

29210 

C1 = 

(B2 - 

29220 

p-^ rs 

<B3 - 1 

29230 

D = ' 

CC2 - C 

29240 

IF ; 

C = 2 T1 

29250 

IF ; 

1 = N2 

29260 

j = isJi = : 

29270 

j = •• 

1 : J1 = ; 

29280 

J = N2:J1 = 

29290 

ZO = 

(W( J1 ) 

29300 

FO = 

FdO) ■ 

D> ) 



<N 
= 0 
N2 


9C00 ; MC 


F ( UU > 
0 


NC / FC:X 


NN 


SOR (DXtUU) » 


/ 

Q = 

TO 

II = I + 1 ;I2 = I + 2 
- FdO) ) / (U<I) - UCIO) ) 

- FCD) / <U(I1> - W(I)> 

- F(I1 > ) / (W< 12) - W(I1 ) > 
B1) / CW<I1) - U(IQ)) 

B2) / CW<I2) - W(I>> 

1) / <U<I2) - UCIO)) 

HEN 29270 
THEN 29280 
11: GOTO 29290 
3s GOTO 29290 
N 

+ U(J) ) / 2 

+ (ZO - WCIO) ) * <B1 + (ZO - 


w(i) ) * cci + czo 


UCID) » 


29310 

OQQOfI 


a = Q + (W(J1) - W(J)) * (F(J1) + F(J) + 4 * FO) / 6 
NEXT I 

29330 PRINT 
29340 RETURN 
29390 REM f i r>d Bezier 
29420 XG = OsYG = 0:NN 
INT CNC) » <(1 - 
sYG = YG + Y(RR 
29490 REM length by 
29500 N = 5 
29510 FOR UU = 2 TO 


HTAB 5s PRINT "VALUE OF THE INTEGRAL = 

Point 

= L2%(KK)s FOR RR = 0 TO L2ZCKK)s GOSUB 29110sPQ = 
- U) (NN - RR)) » (U RR):XG = XG + X<RR,KK) * PQ 
KK) » PO: NEXT RR: RETURN 
i nt egrat i on 


(N - 1):U = (UU - 1) / (N - 1):DX(UU) = 
sNN = L2X(KK>s FOR RR = 0 TO L2X(KK)s GOSUB 29110 
29520 pa = INT (NO- * ((1- U) (NN - RR - 1)) » (U '' (RR 
NN * U>!DX(UU> = DX(UU>'+ X(RR,KK> » PQsDY(UU) = DY(UU) 
'-/'V’' ; P<as NEXT 'RR s'; NEXT', UUi^-'y.; ■ ' 

XC0,KK))5DY(i) = NN (Y(1,KK> 
X<NN - 1,KK))sDY(N) = NN » (Y(NN 


0:DY(UU> = 

D) » (RR 
+ Y(RR,KK) 


0 


,1* NN , , % ' !V.w 

NN « (X(Nn;KR>/^ 


29530 DX(1) 

X(N) » 

1,KK)) ; i 

29540 GOSUB 29'120s RETURN v • * > x p 

30000 REM point within range and outfjdc edge 
30005 PRINT "Point WITHIN range |tnd: W^ 

,10 EO* « NR*t„ GOSUB , 20050 
,i:i . NO . . «> , ( T)^,( UX . 'X0>,,, ■ ^ Z: , ^ 


- Y(0, 
KK) - 



KK) )5D 
Y(NN - 


sMS*;'* 


30020 NO = 
D1% = 


CXO - XP) » CXO - XP> + (YO - YP> » (YO - YP): I- 
D1% + ■1:X(D'1-:,D2%> = TX< JX> ; Y<D1%,D2%) = TY(JZ): 3 


3003 0 D1% 


D-1% + 1 ;X(Dr:,D2%> = EX< J%) : Y<D1X,D2%) = EYC, 


30040 X1 - XP:Y1 = Y?:X2 = X<DI%,D2X) : Y2 = Y ( Di X, D2X ) ; V1 % = 
(.NP. '1 ) ) ;X3 = X3 ( V 1 X) : Y3 = YB( V1X):R1 = RSX<V1X): GOS.' 
% = 1 THEN 30060 


> NO 

OTO 30340 

'-j t cj <3 

VAL < L£FT$ 
Z;3CQ: IF EG 


30CS0 V1X = VAL ( RIGHT! (NS$,1>):X3 = XB(U1X):Y3 = Y3CV:‘.-:E' - R3XCV1X 
)s 30303 25000: IF EUX = 0 -THEN 30080 ■ 

30060 NG = RD(NS%) + RSXCNSX): IF RD<NSX - 1 ) ■+• RSXCNSX - ' > NG THEN NG 

= RDCNSX - 1) -I- RSZ<NS% - 1) 

30070 NH = CAN(NS%> + ANCNSX - 1)) / 2:D1Z = D1X ,+ 1 : X C D1 X . I LX . = X<D1a - 
nD2%) :Y(D1X,D2Z> = YCD1Z - 1,D2X):X<D1X - nOGX) = aO - %3 * 003 < 

NH):Y(D1% - 1,D2%> = YO + NG * SIN <NH) 

30080 P1% = P1% + 1:S1X(?1X) = J%:L2X<D2X> =01% 

30090 IF TM%<JX> < > 3 AND TN%(JX) = 0 THEN P1X = P1% + 1;3r:<?;X) = T.4 

= THXCJX): GOTO 30090 
30093 IF TMX(JZ) = 0 THEN 30100 
30095 GOSUB 20800: GOTO 30090 ■ 

30100 IF FIX = 0 THEN 30310 

30103 GOSUB 20100:KK = 0: GOSUB 20530 

30105 GOSUB 31000 

30110 GOSUB 32000 

30180 GOSUB 33000 

30190 RETURN 

30200 REM point within range and inside edge 
30205 PRINT "Point WITHIN range ar. INSIDE edge" 

30210 EOS = NRS: GOSUB 20050 


30220 IF TMX(JX) = 0 THEN FIX = - 1: GOTO 30330 

30230 JX = TMX(JX>:X1 = X0:Y1 = Y0:V1X = VAL ( LEFTS (EDS ( jX > , 1 ) ) : X2 = X 
8<V1X)!Y2 = YB<V1X>:V2X = VAL C RIGHTS < EDS ( JX ) , 1) > s X3 = X8(V2X):Y3 
= YB(V2X):X4 = XP:Y4 = YP : GOSUB 26150: IF SUX = Q THEN 30240 
30235 GOTO 30220 
30240 P1X = 0:S1X(P1X) = JX 


30280 IF TMX<JX) < > 0 AND TN%CJZ5 = Q THEN P1% = ^1% + ".•E'XJPtX' = TM 

Z(JZ>:JZ = TMZCJX): GOTO 30280 
30290 IF T«X(JZ) = 0 THEN 30310 
30300 GOSUB 20800: GOTO 30280 

30310 IF P1Z = 0 THEN V1Z = VAL ( LEFTS CEDS(S1X<P1Z) > ,1 ) ) :V2Z = VAL < 
RIGHTS (EDS(S1Z(P !Z) > ,1 ) > :A = YB(V1X> - YB(V2X):8 = X8<V2Z> - XB(V1 
%>;C = - YB(V2%> # 8 - XB(V2Z) » A:PD = ABS (A » XP -h 8 » YP + C) / 
SOR (A » A + 8 B); IF PD < 2 * AA(NL%) THEN 30330 

30320 GOSUB 20100:D1X = Q:D2% = 0:KK = 0: GOSUB 20530: GOSUB 310Q0: GOTO 
30110 

30330 PRINT "Extending arrow at edge "jEDSCJZ);" for a point." 

30340 D2Z = 0:L2Z<D2Z> = 1;LH(0> = 0:M = (TY<JZ) - HY(JX)) / <TX(JZ) - HX 
(J%));PO = (HX(JZ) - TX<JX)) 2 + <HY(JZ) - TY<JZ)) '' 2: IF P1X = 0 
THEN X(L2Z(D2X) ,02%) = EX<S1X(P1X)) + SGN (XO - X(0,0)) * 3QR (6 * 


PO / (1 + M » M)): 
30345 X(L2X{D2X),D2X) = 
M n M)) 

30(^50 Y C L2% ( D2% ) , D2% ) =* 

4' ■, a jxt goto 

30500 REM point not in 
30505 PRINT "Point is 


GOTO 30350 
X<0,0) + SGN 


(XO - X(0,0)) * SOR (6 » PO / (1 + 


30510 EQS 
V1X 

30520 VI % 
30530 NH 

**1 9 

■ 1%) 4- 


M * (X(L2%<D2Z),D2Z) 
30110 

natural state 
BEYOND range" 


X<0,0)) + Y<0,0):P1Z = 0:S 


P NRfsGOSUB 20050sPl% 
NP5i8 30T0 30530 ; 

^ , ", I ‘i T-* X 

~'NP%‘''-1' ■ 


- 1sDl% X=, 0 sD2Z 


= Os IF NPZ < NS% THEN 


»* AN< V1%) #Ht3 ;W4 ;+i,; N6. 


RSXCViJjftwtl 


» cos 

2:ND 


(NH) :Y 
= RD(V 



30540 IF NC > ND THEN 3Q590 

30550 X2 = XP;Y2 = YPsX3 = XBCM1%):Y3 = Y3 (Ul %) : Ri = RS% (Ul %) : G0SU3 2400 
0: IF SU% = 1 THEN 3058Q' 

30560 02% = NS% - 1: IF 02% < •> 01% THEN X3 = XB(V2%):Y3 = Y3(V2%):R1 = 

R3%C02%): GOSUB 26000: IF SU% =■ 1 THEN 30580 
30570 02% = NS%: IF 02% ( >01% THEN X3 = X8(V2%):Y3 = YB<02%):R1 = R5%< 

02%) : GOSUB 26000: IF 3U% = 0 THEN 3C570 
30530 HH = CAN(NS%) + AN(N3% - 1)> / '2:01% = D1% + 1:X(D1%,G) = XO + NO * 
COS CNH):Y(D1%,0> = YO + NG * SIN <NH) 


3Q59Q 01% = 01% + 1;X(D1%,0> = X1:YC01%,0> = Y1:X2 = EX( J%) :Y2 = EY(J%>;X 
3 = XB<01%):Y3 = YS(yi%):R1 = RS%<yi%): GOSUB 26000: IF SU% = 1 THEN 
NH = <AN<NP%) + AN(NP% - 1)) / 2:01% = 01% + 1:X<D1%^0) = XO + NG » 
COS (NH);Y(D1%,0) = YO + NG » SIN (NH) 

30600 01% = 01% + 1:X(D1%.0) = TX( J%) :Y(01%,0) = TY(J%):01% = 01% + 1:X(D 
1%,0) - EX( J%) :Y<D1%,0) = EY(J%):P1% = P1% + 1:31%<Pi%) = J%:L2%(D2% 
) = 01%: IF U1% = 1 THEN 30620 
30610 GOTO 30090 

30620 KK =0: GOSUB 29500:LH<KK) = 0; GOTO 30110 
31000 REM integrate the Bezier Curves 
31010 IF 02% = 0 THEN LH(D2%) = 0: RETURN 

31020 FOR KK = 0 TO 02% - 1: GOSUB 29500:LH(KK) = S: NEXT KK: RETURN 

31100 REM find collision cases if any 
31110 II = NL% 

31120 IF II = 0 THEN 31190 
31130 JJ = 1 

31140 IF JJ > 0C% THEN 31180 

31160 X1 = XK(II):Y1 = YK(II):X2 = XKXII - 1):Y2 = YKCII - 1):X3 = XBCJJJ 
;Y3 = YB(JJ):R1 = RS%CJJ) : GOSUB 26000: IF SU% = 1 THEN PRINT "Link 
collides with obstacle "?JJ: GOTO 31190 
31170 JJ = JJ + 1 : GOTO 31140 

31180 II = II - 1: GOTO 31120 

31190 RETURN 

31300 REM find edge of- collision 
31310 KK = P1%:NC = 1.0E20:II = - 1 

31320 IF KK < 0 THEN 31360 

31330 V1% = VAL < LEFT* <ED*(31%<KK) > ,1 ) > :y2% = VAL ( RIGHT* (ED*CS1%(K 
K)),1)): IF yi% = JJ OR 02% = JJ THEN 31350 
31340 KK = KK - 1; GOTO 31320 

31350 ND = (XBCJJ) - EXCS1%(KK))) 2 + (YB(JJ> - EY(S1%(KK>)> 2; IF ND 

< = NC THEN II = KK;NC = ND 

31355 GOTO 31340 

31360 IF II <. 0 THEN GOSUB 31500: RETURN 

31370 MK%<II) = 1s RETURN 

31500 REM mark all edges this time 

31520 FOR II = 0 TO P1%;MK%<II) =1: NEXT II 
31570 RETURN 


32000 REM find last compensatory length 

32010 LE = Os FOR KK = 0 TO D2% - 1:LE = L£ + LH<KK): NEXT KK:LR = L - LE 

32020 L2%<D2X) * L2%(02%>^-^ = X0;Y<L2%<D2X) ,D2X) = YO , 

32030 KK = D^Xa ; GOSUB 29500*1. IF A8S <LR - 0) < 20 THEN LH<KK> - 0: RETURN 

320^0 NC V 0*01% < LEFT«> <EB*(SlX(P1X))r1)>?V2X = VAL < RIGHT* (E 

* " XB-CVIX).- -^'■XBCV2X) a - GOSUB- 

32043 QZ ' tel' ' ^ i ' 

32045 Ml » BpaOX = XO - eX(S1XiF1X)JI?0Y; « T%; “ E^ GOSUB 24470a 

XB<V2X> -•'XO).'V'^■''^' ' >l 

■ ^ 


,32050 L25£(D2X) L2X(D2Xt,,^:t^^^,^. 



32G60 A = EX(S1%<P1S:)) + GZ ♦ A3S CMC - LR ) / SSR (1 + m * • 

(A - EX(S-1%(P 1%) ) > + EY<31%(P1%)> 

32065 IF XA - XK(1)) '' 2 + <3 - YKd).) 2 < (X<L2%<D2%) - 1,D2X) - XKCt 
) ) '■' 2 + (YCL2X(D2X) - 1,D2X) - YXd)) ''2 THEN : X ( L2X ( D2X ) - 1 , D2% ) 

= A:Y(L2%(D2%) - 1,02.-:) = B 

32070 G0SU8 29500: IF ABS (Q - LR) > 10 AND RX < 10 THEN GZ = GZ * LR / 

S: GOTO 32055 
32200 LHCKK) = 0; RETURN 
33000 REM find Joint Coordinates 

33005 PRINT "Number of curves : ";D2% + 1: PRINT "Stack : " ; : FOR ZZ = 0 TO 
P1X; HTA8 9: PRINT ED$CS1%(ZZ) ) ; " "jHKZCZZ): NEXT ZZ 
33007 IF HZ = 1 THEN GOSUB 2Q7Q0 

33010 FOit = FI$ + ".JNT":LE = 0:KK = 0: GOSUB 21500:LJ = LH(KK):II = NL%: 
XK<II) = XP;YK<II) = YP;XL(II) = XPiYLdl) = YPsXKCO) = X0:YK(0) = Y 
0:XL(0) = X0!YL<0) = YO 
33020 IF II = 1 THEN 33080 
33030 LE = LE + AAdI) 

33040 IF LE > LJ THEN 33070 

33050 V = (LE - LJ + LH<KK>) / LH(KK): GOSUB 23000 :U = 0 + PLs GOSUB 2942 
0:XLdI - 1) = XGsYLdI - 1) = YG 
33060 II = II - 1: GOTO 33020 

33070 KK = KK + 1s GOSUB 21500:LJ = LJ + LH<Kid: GOTO 33040 

33080 II = NL% - ^ - 

33090 XM = XPsYM = YPsXG = XLCII - DsYG = YLCII - 1) 

33100 GOSUB 33400 

33120 XKCII - 1) = XIsYKCII - 1) = YI 

33130 II = NLX - 1 

33140 IF II =2 THEN 33200 

33150 XM = XKCII) sYM = YKCIDsXG = XLCII - DsYG = YLCII - 1): GOSUB 3340 
QsDX = XKCII) - XIsDY = YKCII) - YI: GOSUB lOSOOsC = SP - Ml; IF ABS 
CO > PI THEN C = C - SGN CO » 2 » PI 

33155 IF ABS CO < = JLCII + 1 ) THEN M1 = SP : GOTO 33170 

33160 Ml = Ml - SGN CO » JLCII + Ds PRINT "Joint ";II + 1j" is being p 
ut to the limit":M== TAN C M1 ) : GOSUB 33430 
33170 XKCII - 1) = XIsYKCII - 1) = YIsII = II - 1 ; GOTO 33140 
33200 XM = XKC2)sYM = YKC2>:D = CXM - XO) *' 2 + CYM - YO) 2: IF SOR CD 
) > AAC1) + AAC2) THEN GOSUB 33500s GOTO 33260 
33210 K1 = CD + AAC1) * AAC1) - AA(2) » AAC2)) / (2 » AAC1)):K2 = K1 / SSR 
CD): IF C'XM - XO) = 0 THEN AL = PI / 2: GOTO 33230 
33220 AL = ATN CCYM - YO) / CXM - XO)): IF XM < XO THEN AL = AL + PI 

33230 TH = AL + ATN C SQR (1 - K2 K2) / K2):XI = XO + AAC1) * COS CTH 

)sYI = YO + AAC1) * SIN (TH) 

33240 TH = AL - ATN C SOR C1 - K2 » K2) / K2):XK = XO + AAC1) » COS CTH 

)sYK « YO + AAC1) * SIN CTH): IF CXK - XW%) '' 2 + (YK - YUZ) '' 2 < 

CXI - XU%) 2 + CYI - YW%) 2 THEN XI = XKsYI = YK 
33250 XKCD = XIsYKd) = YI ,_, 

33260 IF FX » 1 THEN 33290 
33265 GOSUB 31100a IF II < > ' 

33270 PRINT D0<^: PRINT DO*?''WR 
XK<ZZ)r,''?YK<ZZ)8 NEXT ZZ 
33280 FOR ZZ =* NL% TO 2 STEP ; 

YKCZZ))" TO " INT (XKCZ3|ilT^ 

HPLOT "j INT CXKCI))"^;;!^ 


1: PRINT 



M » CXI 


33430 XI = XH + SGN <XG - XH) * AA(II) / SQR (1 + M * !i) :YI = 

XH) + YH; RETURN 

33500 REM reverse cacalatiGn rcr joints 
335iG PRINT "Going for reverse calculation" 

33540 XG = XL(1):YG. = YL<1):XN = XQsYH = V0:II = 1: GOSUB 33400 ; XK ( 1 ) = X 
I:YK<1) = YI:XM = XK(3):fH f YX(3);D = <XH - XI) '-' 2 + (YM - YI) 2 

33550 K1 = (D + AA(2) * AA<2> - AA(3) Jt AA<3)) / (2 * AAC2)):K2 = K1 / 3QR ! 

(D) : IF (XH - XI) = 0 THEN AL = PI / 2: GOTO 33570 
33560 AL = ATN ((YH - YI) / (XH - XI)): IF XH < XI THEN AL = AL + PI 
33570 IF K2 > 1 THEN K2 = .999: PRINT CHR$ (7): PRINT "Point (" INT (XP 
)'%" INT (YP)") is too far away !": IF C% < 3 THEN F)i = 1: RETURN 

33580 TH = AL + ATN ( SQR (1 - K2 * K2) / K2):XL = XI + AA(2) * COS CTH 

):YL = YI + AA(2) * SIN (TH) 

33590 TH = AL - ATN ( SQR (1 - K2 » K2) / K2):XI = XI + AA(2) # COS (TH 
):YI = YI + AA(2) * SIN (TH): IF (XI - XK(2)) '' 2 + (YI - YK(2)) 

2 < (XL - XK(2)) 2 + (YL - YK(2)) '' 2 THEN XL = XI:YL = YI 

33600 XK (2) = XL:YK(2) = YL: RETURN 
35000 REH Up Solut ion for no obstacles 
35005 PRINT "Seek i ng Up Solution" 

35010 D2X = 0:L2X(D2X) = 3:X(3,D2%) = X0:Y(3,D2%) = Y0:LE = L / .9:LD = SOR 
((XP - XO) 2 + (YP - YO) 2): IF XP = XO THEN M = 9.999E17: GOTO 
35030 

35020 H = (YP - YO) / (XP - XO) 

35030 OP = (LE - LD) / 2:NC = 1:DC = OP / SQR (1 + H * H) : Yd ,D2%) = YCO 
,D2X) + NC » DC:X(1,D2%) = X(0,D2:<) + H » (Y(0,D2%) - Y(1,D2X)) 

35035 B = Y(0,D2%) - NC » DC:A = X(0,D2%) + H » (Y(0,D2X) - B): IF (A - X 

Kd)) 2 + (B - YKd)) 2 < (Xd,D2%) - XK d ) ) '‘‘2 + (Yd,D2%) - Y 

Kd)) 2 THEN Xd,D2%) = AsYd,D2X) = B:NC = - 1 

35040 Y(2,D2%) = Y(3,D2%) + NC » DC:X(2,D2%) = X(3,D2X) + H * (Y(3,D2%) - 

Y(2,D2X));KK = 0: GOSUB 29500 

35050 IF A8S (0 - L) < 10 THEN LH(KK) = 0: GOSUB 33000: RETURN^^ ^ ^ 

35060 LE = LE » L / 0: GOTO 35030 
39990 REM error hand 1 i ng routine 
40000 IF Z1 > 279 THEN Z1 = 279: RESUME 

40010 IF Z1 < 0 THEN Z1 = Q: RESUME 

40020 IF Z2 > 191 THEN Z2 = 191 : RESUME 

40030 IF Z2 < 0 THEN Z2 = 0: RESUME 

55000 PI = 3.14159265:88 = 180 / PI: INPUT "DX = "jDX: INPUT "DY="?DY:60SU 
10500: PRINT "SLOPE ="? INT (SP » B8)" deg": GOTO 55000 
60000 REM clear output file 

60030 FO^ = FItfr + ".JNT": ONERR GOTO 60050 
60040 PRINT CHR$ ( 4 ); "DELETE" ;F0$ 

60050 PRINT CHR$ (4) j "OPEN" sFO$ 

60060 RETURN 
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1 


1 

REM 

»»»»»»■»* if 4 

2 

REM 

* 


3 

REM 

» OBSTACLE AVOIDANCE 

4 ' 

4 

REM 

* PROGRAM 


5 

REM 


#■ 

6 

REM 

* An up am Bagch i 


7 

REM 

Dec 1936 

4 

8 

REM 

* 

■' 4 - 


9 REM 

10 DIM CG%<20> 

■15 PI = 3. 14159265:88 = 1SO / PI 

20 DIM XE(15> ,YE(-15) ,XB< tO) , YSC10) ,XI%(25) ,YI%(25) 

300 GOSUB 9500: G03UB 8200 
320 ONERR GOTO 500 

330 PRINT D0$: GOSUB ■1500:F0S = Fit + ".JNT": PRINT DOt: PRINT DOt?"READ 
"yFOt: ON KEX GOTO 700.300,600. 1000, 900, 350, -1700 
360 PRINT Dot? "CLOSE" ;FOt 
500 IF PEEK (222) = 5 THEN 1400 
510 IF XX( JJ) < 0 THEN XX< JJ) = 0: RESUME 

520 IF XX(JJ) > 279 THEN XX(JJ) = 279: RESUME 

530 IF YY(JJ) < 0 THEN YY(JJ) = 0: RESUME 

540 IF YY(JJ) > 191 THEN YY<JJ) = 191: RESUME 

590 PRINT CHRt (7);: PRINT "Something is wrong !": PRINT : PRINT "EKiti 
ng because of confusion.": PRINT : HTAB 10: PRINT "I am sorry for th 
at.": PRINT : PRINT "Type RUN to start program again.": TEXT : END 
600 REM superposed configurations . 

610 GOSUB 50200: GOSUB 3300 

620 FOR Ki< = 1 TO 400: NEXT KK: FOR JJ = 1 TO NLX + 1- INPUT XX(JJ),YY(J 
J>: NEXT JJ: HPLOT XX<1),YY(1>: FOR JJ = 2 TO NL% + 1: HPLOT TO XXC 
JJ).YY(JJ): NEXT JJ 
630 GOTO 620 

700 REM list points on terminal 
705 IZ = 1 


710 FOR JJ = 1 TO NL% + 1: INPUT XXC JJ) , YY( JJ) : NEXT JJ: PRINT : HTAB 8: 

PRINT "Iteration number ";I%: PRINT : PRINT XXd ) , YY(1 ) :CV = 0 
720 FOR JJ = 2 TO NL% + 1: PRINT XXC JJ) , YY< JJ) :CV = CO + SOR <(XX<JJ) - 
XX(JJ - 1)) 2 + CYY<JJ) - YYCJJ - 1)) 2): NEXT JJ 

730 HTAB 5: PRINT "LENGTH = ";CO:IZ = 1% + 1 : GOTO 710 
800 REM draw configuration one by one 
810 GOSUB 50200: GOSUB 8300 

820 FOR JJ = 1 TO NL% + 1: INPUT XXC JJ) .YYCJJ) : NEXT JJ: HCOLOR= 3 
830 GOSUB 840': GOSUB 8000: HCOLOR= 0: GOSUB 840: GOTO 820 

840 HPLOT XXCD.YYCI): FOR JJ = 2 TO NLZ + 1: HPLOT TO XXC JJ) , YYCJJ) : NEXT 
JJ: RETURN 

850 REM plot endeffector path 
860 GOSUB 50200: GOSUB 8300 


FOR JJ = NLZ TO 0 STEP - 1: INPUT XXC JJ) .YYCJJ) : 
HPLOT XXCNLZ) .YYCNLZ): GOTO 870 
REM calculate, plot & display JOINT COORDINATES 
FOR JJ » NLZ TO 0 STEP - 1| INPUT XXC JJ),;YYUJ): 
)X « XXCD - XX(0)aDY = YY(1> - YYCOJ’s GOSUB 10500 
MB) > PI THEN MB = MB - SON (MB) *2 « PI 


NEXT 


FOR JJ » NLZ TO 0 STEP - 1| INPUT XXC JJ),;YYUJ) : NEX 
DX « XXCD - XX(0)aDY = YYCD - YYCOJ’s GOSUB 10500sMB 
MB) > PI THEN MB = MB - SON (MB) * 2 « PI „ .■ 

PRINT INT (MB » BB » 10 '/;.10, ■ ''"v ’ 

FOR JJ » 2 TO NLZ:M1 = SPsOX = XX(Ji) - XXUJ - 1):DY 
, JJ 1 > * GOSUB 10500»? ■ HTAB’. Jjr.-'-ra'I 4' SB m.':;- 

V IF : ABS (MB ) : ■: > P I THEH3^.* VB '' '' ' “ “ ’ 


JJ),:YY(JJ)s' NEXT 

GOSUB 10500:MB = SP: IF ABS 

» PI ■ /‘-"‘A 




fSO FRIHT SGN <VB) 




MB) * IN' 


1000 REM draw Joint angles 

•1010 PRIN"^ DOli: PRINT : PRINT "Which joint angle do you want to plot ?": 

PRINT : PRINT : HTAB 10: INPUT 'ANGLE NUMBER = ";KX 
1012 PRINT : PRINT : HTA8 10: INPU'' '*'<AGNIFICATI0N = ";HA: PRINT : PRINT 
: HTA3 10s INPUT "STEP LENGTH = ';ICX 
1015 ^RINT D0T>?"READ":F0$ 

1020 HGR2 : HC0L0R= 3:1% = OsMDX = IGQ: ONERR 3010 13-30 

1030 riPLOT 0,191 TO 0,0 TO 279,0: FOR JJ = 10 TO 270 STEP 10: HPLOT JJ,0 
TO Ju,3: NEXT : FOR JJ = 10 TO 180 STEP 10: HPLOT Q,JJ TO 3,JJ: NEXT 


1035 HPLOT 279, HDX TO 0,HD%:XI% = OjYIX = MDX 

1040 FOR JJ = NL% TO 0 STEP - 1: INPUT XXCJJ) , YY< JJ) ; NEXT JJ 
1050 IF K% < 1 OR KX > NL% THEN 11G0 
1060 IF KX = 1 THEN 1090 

1070 DX = XXCKX - 1) ” XX<KX - 2>:DY = YY(KX - 1) -'YY(K% - 2) : QOSUB 105 
00sM1 = SPsDX = XX<K%) - XX(K% - 1);DY = YY(K%) - YY<K% -1): GOSUB 
10500:VB = SP - M1 

1075 IF ABS (UB) > PI THEN VB = VB - SGN (V8) i* 2 * PI 

1080 XJX = ICX * IXsYJX = VB * MA + MD%: HPLOT XI%,YI% TO XJ%,YJ7.:XI% = X 



;E MODE OF DISPLAY"! VTAB 4! GOSUB 9000! VTAB 
PRINT "1 to List Coordinates on screen 
" Draw cdilf Iguhation one by one 
Draw superposed conf igurat ions" 
dinates wrt tiwe": PRINT "S t 


PRINT i PRINT 
List Jo I 


1090 

1095 

1100 

1105 

1110 


1115 

1120 


1300 

1310 

1320 

1330 

1340 

1350 

1360 

1400 

1410 

1420 

1430 

1433 

1435 

1437 

1440 

1450 

1460 


GOTO 1040 
1 > :DY = YY<KX) - YY(KX - 
THEN VB = VB - SGN (VB) 


JX:YI% = YJX:I% = IX 
DX = XX(KX) - XXCKX - 
SP: IF ABS (VB) > PI 
GOTO 1080 

DX = XXCI) - XX(0):DY 
I THEN SP ~ SP - SGN 
JC ( 1 ) = SP 
FOR CC = 2 TO NLXsMI 
<CC - 1): GOSUB 10500 
C(CC) = JC(CC) ~ SGN 
NEXT CC 

XJX = ICX » IX; FOR CC = 1 
,JM(CC) TO XJX,YJX:JM(CC) 

1040 

PEEK (222) = 5 THEN 1400 

0 THEN YJX = 0: RESUME 
191 THEN YJX » 191: RESUME 
279 THEN XJX = 279s RESUME 
0 THEN XJX = 0: RESUME 


GOSUB 
» PI 


10500:VB 


= YYd ) ■ 
(SP) « 2 

= SP:DX = 
iJCCCC) = 

( JC(CC) ) 


YYCO) 

* PI 

XXCCC) 
S? - M1: 
» 2 * PI 


- XX<CC - 1):DY = YY(CC) - YY 
IF ABS (JC(CC)) > PI THEN J 


TO NLXjYJX 
YJX: NEXT 


= JC(CC) 
CC;XI% = 


» MA + MDX: 
XJXsIX = 1% 


HPLOT XI% 
+ 1; GOTO 


IF 

IF 

IF 

IF 

IF 


< 

> 

> 

< 


YJX 
YJX 
XJX 
XJX 
RESUME 
GOTO 590 
REM asks 
SK 
KK 


for a 
- 16336!X 
RND <1) * 


choice of replay 
- PEEK (SK) + PEEK CSK) 
255: FOR I = 1 TO KK: NEXT 


KE =» PEEK ( - 16384)! IF KE < 128 THEN 1410 
IF KE < > 155 THEN 1440 

POKE - 16368,0 

KE « PEEK < - 16384): IF KE < 128 THEN 1437^ 
POKE - 16368,0! TEXT = HOME * VTAB 6:A$ = 


Do you want to run prog 


PRINT 


PRINT "7 


•1525 PRINT : PRINT -'6 to Plot End Effector path 
to Draw selected conf I gurat i on-s 
■1530 GET [<E£:<£% = A3C (KETO: IF KEZ i 49 OR KE% > 55 THEN ■1530-1540KE% = 
KE% - t3 • 

•1540 KE% = >;Z% - 48 • ^ , 

•1590 RETURN 

•1700 REN plot only selected configurations 

•17'10 PRINi D0>; HOME : VTAB 5:Ai^ = "Enter the conf i gurat i on nurabers" : GOoSJS 
9000: PRINT :Ai = 'you want t o p 1 ot : G0SU8 9000: PRINT ; PRINT : HTAB 
5: INPUT "Number of configurations = ";NMX 
•1720 FOR K:< = 1 TO NH%: PRINT : HTAB 2: PRINT "< ‘"yKK?" > Configuratio 
n number = INPUT "";CG%<KK>: NEXT KK 

■1725 G0SU8 502G0: GOSUB 8300 
•1730 PRINT D0$; ''READ";F0$:IX = -1 

•1740 FOR JJ = 1 TO NLX •*■ Is INPUT XX( JJ) , YY( JJ) s NEXT JJ 

•1750 GOSUB 1900: IF SUX = 1 THEN HPLOT XX(-1 > /YYC'I ) ; FOR JJ = 2 TO NLX •+ 

•1: HPLOT TO XX( JJ) /YY< JJ) : NEXT JJ 
1760 IX = IX 1: GOTO 1740 

1900 REM tells whether present conf i gur at i on needs to be plotted 
•1910 SUX = Os JJ = 0 

1920 JJ = JJ -i- 1; IF JJ > NMX THEN SUX = 0: RETURN 
1930 IF CGX(JJ) =‘IX THEN SUX = 1; RETURN 
1940 GOTO 1920 

2100 REM read network from disk 

2110 TEXT : HOME : VTAB 6: PRINT "Which file do you want to operate upon 
?"; 

2120 VTAB 9: HTAB 10: INPUT "FILE NAME ==> ";FI$: IF LEN <FI$> = O THEN - 
2120 

2130 IF LEN <FI$) > 4 AND ( RIGHTS <FI$, 4) = ".DAT" OR RIGHT':? (FIi?,4) = . 

".NET" OR RIGHTS (FI$,4) = ".PIC" OR RIGHT$ (FI$,4) = ".JNT") THEN 
FI^ = LEFT$ (FI$, LEN (FI$) - 4) 

2140 F0$ = FI$ ".NET" 

2150 D0$ = CHR$ (4): PRINT DOf: PRINT D0$; "READ" ;F0$ 

2160 INPUT XO,YO,EC%: FOR JJ = 1 TO ECX; INPUT XE( JJ) , YE( JJ) : NEXT JJ 
2170 INPUT OCX: FOR JJ =1 TO OCX: INPUT XB( JJ) , YB< JJ) ,RSX< JJ) ,R0< JJ) ,AN 
<JJ): NEXT JJ; INPUT NLXsL = 0: FOR JJ = 1 TO NLX: INPUT AA(JJ>sL = 

L + AACJJ); NEXT JJ 

2180 INPUT EDXs FOR JJ = 1 TO EDX: INPUT ED$( JJ) ,TMX<JJ) ,TN%CJJ) ,HXCJJ) , 

HY( JJ) ,TX<JJ) ,TYCJJ),EX< JJ) ,EY<JJ) : NEXT JJ 
2190 PRINT D0*;"CL0SE";F0$s RETURN 

8000 KEX = PEEK ( - 16384): IF KEX < 128 THEN 8000 
8010 POKE - 16368,0: RETURN 
8200 REM read picture from disk 
8210 F0$ = FI* + ".PIC" 

8230 DO* = CHR* <4); PRINT DO*; "READ" ;F0*: INPUT KIXs FOR KK = 1 TO KIX: 

INPUT XIX(KK) ,YIX(KK) : NEXT KK: PRINT DO*; "CLOSE" ;F0* 

8240 RETURN 

8300 REM draw picture 

8310 IF KIX = 0 THEN RETURN 

8320 FOR II = 1 TO KIX: IF YlXdl) > 1000 THEN HPLOT XIXI II ) , YIX< 1 1 ) - 
1000: GOTO 8340 

8330 HPLOT TO XIX< ID , YIX< II ) 



8340 

NEXT II 





3350 

RETURN 





9000 

B = INT (20 - < LEN (A*)- 4 ' 2)): IF 

B = 

< 0 THEN 

B = 1 


9010 

HTAB B; PRINT A*: RETURN 





9500 

REM read -stored data 





9510 

TEXT : HOME : UTAH 6 : PRINT "Which 

f i le 

do you want to operate 

up on 

9520 

T 

UTAB 9s HTAB 1Qs INPUT "FILE NAME = 

;=> " 

;FI*; IF 

LEN (FI*) = 0 

THEN 


9520 

9530 IF LEN (FI*) > 4 AND ( RIGHT* (FI*, 4) = ".DAT" OR RIGHT* (FI*, 4) = 
".NET" OR RIGHT* (FI*, 4) = "-PIC" OR RIGHT* (FI*,4) = ".JNT") THEN 
FI* = LEFT* (FI*, LEN (FI*> - 4) 

9540 FO* = FI* + ".DAT" 

9550 DO* = CHR* (4): PRINT DO*: PRINT DO*? "READ" ?F0*: INPUT XO,yQ,ECX: FOR 
KK = 1 TO EC%: INPUT XE(l<K > , YE(KK ) : NEXT KK: INPUT OCX: FOR KK = 1 TO 
OCXs INPUT XB(KK) ,Y3(KK) ,RSX<KK): NEXT KK 
9560 INPUT NLX:L = 0: FOR KK = 1 TO NLX: INPUT AA(KK):L = L + AA(KK>: NEXT 
KK; PRINT DO* ; "CLOSE" ; FO* 

9570 RETURN 

■10500 REM calculates THETA from vector 
•10520 IF DX = 0 THEN SP = P I / 2s GOTO 10540 
■10530 SP = ATN (DY / DX) 

■10540 IF DX < 0 THEN SP = SP ■^ PI 
■10550 RETURN 

50000 REM print all information 

50010 FOR II = 1 TO OCX; PRINT "XB("?II?") = "?X 8 (II)?" Y 8 ( " ; I I ? " ) = " ?Y ^ 

B < 1 1 ) s NEXT II 

50020 GEt'kE*: FOR II = 1 TO OCX: PRINT "RSX("?II?") = "?RS%(II); NEXT I 
I; GET KE*: FOR II = 1 TO EDX: PRINT "ED*(";II;") = ";ED*(II);" THX( 
";II;") = "sTMXdD?" TNX("?II;") = "?TNX(II): PRINT "HX( " ; II ? " ) = " 
;HX(II)?" HY("?II;") = ";HY(II) 

50030 PRINT "TX("?II?") = "?TX(II)?" TY("?II?") = "?TY(II)s PRINT "EX("? 

II?") = ";EXCII)?" EY("?II?") = ";EY(II): NEXT II 
50040 RETURN 

50100 REM print input data 

50110 TEXT ; HOME s OTAB 5s HTAB 5: PRINT "Starting point s ("?XO?",";YO 
;")": PRINT s HTAB 7s PRINT "Number of end points = "?ECX: FOR JU = 

1 TO ECXs HTAB 7s PRINT "End point "?JJ?" at ( " ? XE( JJ) ? " , " ? YE( JJ) ; " ) 


"s NEXT JJ ' 

50120 PRINT : HTAB 7; PRINT "Number of obstacles = "?OCXs FOR JJ = 1 TO 
OCXs HTAB 3s PRINT "Obstacle "?JJ?" at ( " ?XB ( JJ) ? " , " ? YB( JJ) ; " ) with 



"sNLXsL 
= "?AA(JJ 
Ls RETURN 


50200 REM plot the points 

50210 HGR2 s HCOLOR= 3; GOSUB 50300 

50220 FOR JJ = 1 TO. OCX: GOSUB 50340s NEXT JJ 

50290 R ETURN ‘ ' ■''' 

50300 HPLOT '"0,191^' T0':'0,a-i'T0';279,0:"' •’ 

50310 HPLOT 2?^3»3 ,T0"279,1Qtf HPLOT 279,3 T0 273,10s HPLOT 3,104 TO 7,-188 

8 HPLOT ^ ^ "y- ' , ■ -■; 

5Q320^~ FOP^ 44L^»li!40;l„T0:''2^ MPtOT yj,0 TO JJ,3!, NEXT'S FOR"JJ “ 10 

TO 480 8'TEJ>;'''TO»:^HPLOT^.o;^J 4..-TO' 3, J41^.,^NeXT,';v^■is■',•i,.'■■4■«rJ 

50330' RETURN ' Jl' 

50340 FOR KK - 0 JO 340 STEP 


RETURN 



{r}E: - 





